CPD Results

The following document contains the results of PMD's CPD 6.55.0.

Duplications

File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2099
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2063
final KnownBiasTurntableGyroscopeCalibratorListener listener) {
        this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known x-coordinate of gyroscope bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known y-coordinate of gyroscope bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY known y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets known z-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known z-coordinate of gyroscope bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     *
     * @return known x-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known x-coordinate of gyroscope bias.
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     *
     * @return known y-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param biasY known y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets known z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return known z-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known z-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known z-coordinate of gyroscope bias.
     *
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in radians
     * per second (rad/s).
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @param biasY known y-coordinate of gyroscope bias.
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of gyroscope.
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @param biasY known y-coordinate of gyroscope bias.
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
        this.biasY = convertAngularSpeed(biasY);
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of gyroscope bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known gyroscope bias  as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @return known gyroscope bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known gyroscope bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param bias known gyroscope bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return turntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        this.turntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
        result.setValue(turntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1022
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2103
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2067
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx, accelerometerMyz,
                accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return x-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return y-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY y-coordinate of gyroscope known  bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return z-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @return x-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @return y-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     *
     * @param biasY y-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @return initial z-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     *
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in
     * radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of gyroscope.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
        this.biasY = convertAngularSpeed(biasY);
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of gyroscope known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets gyroscope known bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets gyroscope known bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param bias gyroscope known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1018
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1021
final Matrix accelerometerMa, final KnownBiasEasyGyroscopeCalibratorListener listener) {
        this(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa);
        this.listener = listener;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx, accelerometerMyz,
                accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return x-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return y-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY y-coordinate of gyroscope known  bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return z-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @return x-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @return y-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     *
     * @param biasY y-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @return initial z-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     *
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in
     * radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of gyroscope.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
        this.biasY = convertAngularSpeed(biasY);
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of gyroscope known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets gyroscope known bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets gyroscope known bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets gyroscope known bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets gyroscope known bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param bias gyroscope known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2103
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1025
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2067
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known x-coordinate of gyroscope bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known y-coordinate of gyroscope bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY known y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets known z-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return known z-coordinate of gyroscope bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     *
     * @return known x-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known x-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known x-coordinate of gyroscope bias.
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     *
     * @return known y-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known y-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param biasY known y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets known z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return known z-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known z-coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known z-coordinate of gyroscope bias.
     *
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in radians
     * per second (rad/s).
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @param biasY known y-coordinate of gyroscope bias.
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of gyroscope.
     *
     * @param biasX known x-coordinate of gyroscope bias.
     * @param biasY known y-coordinate of gyroscope bias.
     * @param biasZ known z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
        this.biasY = convertAngularSpeed(biasY);
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of gyroscope bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known gyroscope bias  as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @return known gyroscope bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known gyroscope bias as a column matrix.
     * Matrix values are expressed in radians per second (rad/s).
     *
     * @param bias known gyroscope bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1051
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2115
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(initialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(initialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(initialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix with values expressed in radians per second (rad/s).
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @return initial bias coordinates.
     */
    public AngularSpeedTriad getInitialBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialBiasAsTriad(AngularSpeedTriad result) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1052
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2154
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(initialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(initialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(initialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @return initial bias coordinates.
     */
    public AngularSpeedTriad getInitialBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialBiasAsTriad(AngularSpeedTriad result) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2111
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2150
final RobustTurntableGyroscopeCalibratorListener listener) {
        this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(initialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(initialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(initialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1048
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1047
final Matrix accelerometerMa, final EasyGyroscopeCalibratorListener listener) {
        this(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa);
        this.listener = listener;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(initialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(initialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(initialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1052
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2115
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(initialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(initialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(initialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1051
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2154
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias ot be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find
     * a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(initialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(initialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param result instance where result data will be stored.
     */
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(initialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a
     * solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial gyroscope bias.
     */
    public double[] getInitialBias() {
        final double[] result = new double[BodyKinematics.COMPONENTS];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2239
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 497
this(measurements, commonAxisUsed, initialBias, initialMg, initialGg);
        this.listener = listener;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial x-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getInitialBiasAngularSpeedX() {
        return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(initialBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial x-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial y-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getInitialBiasAngularSpeedY() {
        return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(initialBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial y-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAngularSpeed(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @return initial z-coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getInitialBiasAngularSpeedZ() {
        return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(initialBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial z-coordinate of gyroscope bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution
     * expressed in radians per second (rad/s).
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of gyroscope bias.
     * @param initialBiasY initial y-coordinate of gyroscope bias.
     * @param initialBiasZ initial z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {

        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as a column matrix
     * with values expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @return initial bias coordinates.
     */
    @Override
    public AngularSpeedTriad getInitialBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final AngularSpeedTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1117
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1073
this(convertAcceleration(groundTruthGravityNorm), measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(biasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(biasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAcceleration(biasY);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(biasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets z-coordinate of known accelerometer bias to be used to find a solution.
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known bias coordinates of accelerometer expressed in meters
     * per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of accelerometer.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
        this.biasY = convertAcceleration(biasY);
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @return known accelerometer bias.
     */
    @Override
    public AccelerationTriad getBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known accelerometer bias.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBias(final AccelerationTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
        biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
        biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets ground truth gravity norm to be expected at location where measurements have been made,
     * expressed in meter per squared second (m/s^2).
     *
     * @return ground truth gravity norm or null.
     */
    public Double getGroundTruthGravityNorm() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2730
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 486
this(measurements, commonAxisUsed, magneticModel, initialHardIron, initialMm);
        this.listener = listener;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronX() {
        return initialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronY() {
        return initialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronZ() {
        return initialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
        this.initialHardIronY = initialHardIronY;
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard iron coordinates of magnetometer used to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
            final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @return initial hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                initialHardIronX, initialHardIronY, initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
                MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial hard-iron used to find a solution.
     *
     * @param initialHardIron initial hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
        initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
        initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialHardIronX;
        result[1] = initialHardIronY;
        result[2] = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialHardIronX = initialHardIron[0];
        initialHardIronY = initialHardIron[1];
        initialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialHardIronX);
        result.setElementAtIndex(1, initialHardIronY);
        result.setElementAtIndex(2, initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix with values expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialHardIronX = initialHardIron.getElementAtIndex(0);
        initialHardIronY = initialHardIron.getElementAtIndex(1);
        initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2722
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 480
this(measurements, commonAxisUsed, magneticModel, hardIron, initialMm);
        this.listener = listener;
    }

    /**
     * Gets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return x-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX x coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return y-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY y coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return z-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronZ z coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed
     * in Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias.
     *
     * @param hardIron magnetometer hard-iron bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 3758
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1824
}

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(biasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(biasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAcceleration(biasY);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(biasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets z-coordinate of known accelerometer bias to be used to find a solution.
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known bias coordinates of accelerometer expressed in meters
     * per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of accelerometer.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
        this.biasY = convertAcceleration(biasY);
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @return known accelerometer bias.
     */
    @Override
    public AccelerationTriad getBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known accelerometer bias.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBias(final AccelerationTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
        biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
        biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4086
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2041
}

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
        return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasXAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasYAsAcceleration() {
        return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasYAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAcceleration(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasZAsAcceleration() {
        return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasZAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution
     * expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final Acceleration initialBiasX, final Acceleration initialBiasY,
                               final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
        this.initialBiasY = convertAcceleration(initialBiasY);
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @return initial bias coordinates.
     */
    @Override
    public AccelerationTriad getInitialBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final AccelerationTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as a column matrix with
     * values expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2724
}

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX known x-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY known y-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in meters Teslas (T).
     *
     * @param hardIronZ known z-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed in
     * Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronY y-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronZ z-coordinate of magnetometer
     *                  known hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return known hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias as a column matrix.
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1194
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2732
}

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronX() {
        return initialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronY() {
        return initialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronZ() {
        return initialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
        this.initialHardIronY = initialHardIronY;
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard iron coordinates of magnetometer used to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
            final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @return initial hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                initialHardIronX, initialHardIronY, initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
                MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial hard-iron used to find a solution.
     *
     * @param initialHardIron initial hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
        initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
        initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron  bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialHardIronX;
        result[1] = initialHardIronY;
        result[2] = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialHardIronX = initialHardIron[0];
        initialHardIronY = initialHardIron[1];
        initialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialHardIronX);
        result.setElementAtIndex(1, initialHardIronY);
        result.setElementAtIndex(2, initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix with values expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialHardIronX = initialHardIron.getElementAtIndex(0);
        initialHardIronY = initialHardIron.getElementAtIndex(1);
        initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 488
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1302
}

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronX() {
        return initialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronY() {
        return initialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronZ() {
        return initialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
        this.initialHardIronY = initialHardIronY;
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard iron coordinates of magnetometer used to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
            final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @return initial hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                initialHardIronX, initialHardIronY, initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
                MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial hard-iron used to find a solution.
     *
     * @param initialHardIron initial hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
        initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
        initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialHardIronX;
        result[1] = initialHardIronY;
        result[2] = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialHardIronX = initialHardIron[0];
        initialHardIronY = initialHardIron[1];
        initialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialHardIronX);
        result.setElementAtIndex(1, initialHardIronY);
        result.setElementAtIndex(2, initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix with values expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialHardIronX = initialHardIron.getElementAtIndex(0);
        initialHardIronY = initialHardIron.getElementAtIndex(1);
        initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 482
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1299
}

    /**
     * Gets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return x-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX x coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return y-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY y coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return z-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronZ z coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed
     * in Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias.
     *
     * @param hardIron magnetometer hard-iron bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1136
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1298
}
    }

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX known x-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY known y-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in meters Teslas (T).
     *
     * @param hardIronZ known z-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed in
     * Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronY y-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronZ z-coordinate of magnetometer
     *                  known hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return known hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias as a column matrix.
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1193
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1301
}
    }

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronX() {
        return initialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronY() {
        return initialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronZ() {
        return initialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
        this.initialHardIronY = initialHardIronY;
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard iron coordinates of magnetometer used to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
            final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @return initial hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                initialHardIronX, initialHardIronY, initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
                MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial hard-iron used to find a solution.
     *
     * @param initialHardIron initial hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
        initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
        initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron  bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialHardIronX;
        result[1] = initialHardIronY;
        result[2] = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialHardIronX = initialHardIron[0];
        initialHardIronY = initialHardIron[1];
        initialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialHardIronX);
        result.setElementAtIndex(1, initialHardIronY);
        result.setElementAtIndex(2, initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix with values expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialHardIronX = initialHardIron.getElementAtIndex(0);
        initialHardIronY = initialHardIron.getElementAtIndex(1);
        initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 482
}

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX known x-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY known y-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in meters Teslas (T).
     *
     * @param hardIronZ known z-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed in
     * Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronY y-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronZ z-coordinate of magnetometer
     *                  known hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return known hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias as a column matrix.
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1194
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 488
}

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronX() {
        return initialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronY() {
        return initialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronZ() {
        return initialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
        this.initialHardIronY = initialHardIronY;
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard iron coordinates of magnetometer used to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
            final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @return initial hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                initialHardIronX, initialHardIronY, initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
                MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial hard-iron used to find a solution.
     *
     * @param initialHardIron initial hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
        initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
        initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron  bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialHardIronX;
        result[1] = initialHardIronY;
        result[2] = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialHardIronX = initialHardIron[0];
        initialHardIronY = initialHardIron[1];
        initialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialHardIronX);
        result.setElementAtIndex(1, initialHardIronY);
        result.setElementAtIndex(2, initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix with values expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialHardIronX = initialHardIron.getElementAtIndex(0);
        initialHardIronY = initialHardIron.getElementAtIndex(1);
        initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2732
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1302
}

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronX() {
        return initialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronY() {
        return initialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronZ() {
        return initialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
        this.initialHardIronY = initialHardIronY;
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard iron coordinates of magnetometer used to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
            final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @return initial hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                initialHardIronX, initialHardIronY, initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
                MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial hard-iron used to find a solution.
     *
     * @param initialHardIron initial hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
        initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
        initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialHardIronX;
        result[1] = initialHardIronY;
        result[2] = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialHardIronX = initialHardIron[0];
        initialHardIronY = initialHardIron[1];
        initialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialHardIronX);
        result.setElementAtIndex(1, initialHardIronY);
        result.setElementAtIndex(2, initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix with values expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialHardIronX = initialHardIron.getElementAtIndex(0);
        initialHardIronY = initialHardIron.getElementAtIndex(1);
        initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2724
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1299
}

    /**
     * Gets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return x-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX x coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return y-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY y coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return z-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronZ z coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed
     * in Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias.
     *
     * @param hardIron magnetometer hard-iron bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 3758
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1824
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1118
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1074
}

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(biasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(biasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAcceleration(biasY);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(biasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets z-coordinate of known accelerometer bias to be used to find a solution.
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known bias coordinates of accelerometer expressed in meters
     * per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of accelerometer.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
        this.biasY = convertAcceleration(biasY);
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @return known accelerometer bias.
     */
    @Override
    public AccelerationTriad getBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known accelerometer bias.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBias(final AccelerationTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
        biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
        biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1136
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1093
this(convertAcceleration(groundTruthGravityNorm), measurements, commonAxisUsed, initialBias, initialMa,
                listener);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
        return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasXAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasYAsAcceleration() {
        return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasYAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAcceleration(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasZAsAcceleration() {
        return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasZAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution
     * expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(
            final Acceleration initialBiasX, final Acceleration initialBiasY, final Acceleration initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
        this.initialBiasY = convertAcceleration(initialBiasY);
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @return initial bias coordinates.
     */
    @Override
    public AccelerationTriad getInitialBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     */
    @Override
    public void setInitialBias(final AccelerationTriad initialBias) {
        initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * This is only taken into account if non-linear preliminary solutions are used.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets ground truth gravity norm to be expected at location where measurements have been made,
     * expressed in meter per squared second (m/s^2).
     *
     * @return ground truth gravity norm or null.
     */
    public Double getGroundTruthGravityNorm() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2724
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 482
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1299
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1264
}

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX known x-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY known y-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in meters Teslas (T).
     *
     * @param hardIronZ known z-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed in
     * Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronY y-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronZ z-coordinate of magnetometer
     *                  known hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return known hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias as a column matrix.
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1194
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2732
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 488
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1302
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1267
}

    /**
     * Gets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronX() {
        return initialHardIronX;
    }

    /**
     * Sets initial x-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
    }

    /**
     * Gets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronY() {
        return initialHardIronY;
    }

    /**
     * Sets initial y-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = initialHardIronY;
    }

    /**
     * Gets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in Teslas (T).
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getInitialHardIronZ() {
        return initialHardIronZ;
    }

    /**
     * Sets initial z-coordinate of magnetometer hard-iron bias to be used
     * to find a solution.
     * This is expressed in meters Teslas (T).
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial x-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial y-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @return initial z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(initialHardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial z-coordinate of magnetometer hard iron bias to be used
     * to find a solution.
     *
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias coordinates of magnetometer used to find
     * a solution expressed in Teslas (T).
     *
     * @param initialHardIronX initial x-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronY initial y-coordinate of magnetometer
     *                         hard-iron bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer
     *                         hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialHardIronX = initialHardIronX;
        this.initialHardIronY = initialHardIronY;
        this.initialHardIronZ = initialHardIronZ;
    }

    /**
     * Sets initial hard iron coordinates of magnetometer used to find a solution.
     *
     * @param initialHardIronX initial x-coordinate of magnetometer bias.
     * @param initialHardIronY initial y-coordinate of magnetometer bias.
     * @param initialHardIronZ initial z-coordinate of magnetometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(
            final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
            final MagneticFluxDensity initialHardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
        this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
        this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @return initial hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                initialHardIronX, initialHardIronY, initialHardIronZ);
    }

    /**
     * Gets initial hard-iron used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
                MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets initial hard-iron used to find a solution.
     *
     * @param initialHardIron initial hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
        initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
        initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getInitialHardIron(result);
        return result;
    }

    /**
     * Gets initial hard-iron  bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getInitialHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialHardIronX;
        result[1] = initialHardIronY;
        result[2] = initialHardIronZ;
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialHardIronX = initialHardIron[0];
        initialHardIronY = initialHardIron[1];
        initialHardIronZ = initialHardIron[2];
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @return initial hard-iron bias to be used to find a solution as a
     * column matrix.
     */
    @Override
    public Matrix getInitialHardIronAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getInitialHardIronAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial hard-iron bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialHardIronAsMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialHardIronX);
        result.setElementAtIndex(1, initialHardIronY);
        result.setElementAtIndex(2, initialHardIronZ);
    }

    /**
     * Sets initial hard-iron bias to be used to find a solution as a column
     * matrix with values expressed in Teslas (T).
     *
     * @param initialHardIron initial hard-iron bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialHardIronX = initialHardIron.getElementAtIndex(0);
        initialHardIronY = initialHardIron.getElementAtIndex(1);
        initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 471
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1138
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1094
}

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
        return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasXAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasYAsAcceleration() {
        return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasYAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAcceleration(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasZAsAcceleration() {
        return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasZAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution
     * expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final Acceleration initialBiasX, final Acceleration initialBiasY,
                               final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
        this.initialBiasY = convertAcceleration(initialBiasY);
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @return initial bias coordinates.
     */
    @Override
    public AccelerationTriad getInitialBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     */
    @Override
    public void setInitialBias(final AccelerationTriad initialBias) {
        initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * This is only taken into account if non-linear preliminary solutions are used.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2100
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2059
&& groundTruthGravityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFy() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFz() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @return estimated accelerometer bias or null if not available.
     */
    @Override
    public AccelerationTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated accelerometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated x coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFxVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFxStandardDeviation() {
        final var variance = getEstimatedBiasFxVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND) :
                null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFxStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated y coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFyVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFyStandardDeviation() {
        final var variance = getEstimatedBiasFyVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFyStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated z coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFzVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFzStandardDeviation() {
        final var variance = getEstimatedBiasFzVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFzStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @return standard deviation of estimated accelerometer bias coordinates.
     */
    public AccelerationTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                getEstimatedBiasFxStandardDeviation(),
                getEstimatedBiasFyStandardDeviation(),
                getEstimatedBiasFzStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of accelerometer bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasFxStandardDeviation(),
                    getEstimatedBiasFyStandardDeviation(),
                    getEstimatedBiasFzStandardDeviation(),
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates expressed
     * in meters per squared second (m/s^2).
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null ? (getEstimatedBiasFxStandardDeviation()
                + getEstimatedBiasFyStandardDeviation() + getEstimatedBiasFzStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null.
     */
    public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasStandardDeviationAverage(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias coordinates.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of accelerometer bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinimumRequiredMeasurements()}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        this.preliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2200
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2333
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return estimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (estimatedHardIron != null) {
            System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedHardIron != null) {
            result.fromArray(estimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return estimatedHardIron != null ? estimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return estimatedHardIron != null ? estimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return estimatedHardIron != null ? estimatedHardIron[2] : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @return x coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[0]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @return y coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[1]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @return z coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[2]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @return estimated magnetometer bias or null if not available.
     */
    @Override
    public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
        return estimatedHardIron != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
                : null;
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
        if (estimatedHardIron != null) {
            result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets variance of estimated x coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated x coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronXStandardDeviation() {
        final var variance = getEstimatedHardIronXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronXStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated y coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronYStandardDeviation() {
        final var variance = getEstimatedHardIronYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronYStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated z coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronZStandardDeviation() {
        final var variance = getEstimatedHardIronZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronZStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @return standard deviation of estimated magnetometer bias coordinates.
     */
    public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
        return estimatedCovariance != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                getEstimatedHardIronXStandardDeviation(),
                getEstimatedHardIronYStandardDeviation(),
                getEstimatedHardIronZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of magnetometer bias was available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedHardIronXStandardDeviation(),
                    getEstimatedHardIronYStandardDeviation(),
                    getEstimatedHardIronZStandardDeviation(),
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates
     * expressed in Teslas (T).
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public Double getEstimatedHardIronStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
                + getEstimatedHardIronZStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of magnetometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationAverage());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias expressed in
     * Teslas (T).
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public Double getEstimatedHardIronStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedHardIronXVariance() + getEstimatedHardIronYVariance()
                + getEstimatedHardIronZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of magnetometer bias
     * is available, false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationNorm());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.preliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1514
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4065
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1453
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2111
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2070
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFy() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFz() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @return estimated accelerometer bias or null if not available.
     */
    @Override
    public AccelerationTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated accelerometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated x coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFxVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFxStandardDeviation() {
        final var variance = getEstimatedBiasFxVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFxStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated y coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFyVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFyStandardDeviation() {
        final var variance = getEstimatedBiasFyVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFyStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated z coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFzVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFzStandardDeviation() {
        final var variance = getEstimatedBiasFzVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFzStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @return standard deviation of estimated accelerometer bias coordinates.
     */
    public AccelerationTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                getEstimatedBiasFxStandardDeviation(),
                getEstimatedBiasFyStandardDeviation(),
                getEstimatedBiasFzStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of accelerometer bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasFxStandardDeviation(),
                    getEstimatedBiasFyStandardDeviation(),
                    getEstimatedBiasFzStandardDeviation(),
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates expressed
     * in meters per squared second (m/s^2).
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasFxStandardDeviation() + getEstimatedBiasFyStandardDeviation()
                + getEstimatedBiasFzStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null.
     */
    public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasStandardDeviationAverage(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias coordinates.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of accelerometer bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(
            final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1479
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2200
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2333
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return estimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (estimatedHardIron != null) {
            System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedHardIron != null) {
            result.fromArray(estimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return estimatedHardIron != null ? estimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return estimatedHardIron != null ? estimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return estimatedHardIron != null ? estimatedHardIron[2] : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @return x coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[0]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @return y coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[1]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @return z coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[2]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @return estimated magnetometer bias or null if not available.
     */
    @Override
    public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
        return estimatedHardIron != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2]) : null;
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
        if (estimatedHardIron != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2], MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets variance of estimated x coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated x coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronXStandardDeviation() {
        final var variance = getEstimatedHardIronXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronXStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated y coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronYStandardDeviation() {
        final var variance = getEstimatedHardIronYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronYStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated z coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronZStandardDeviation() {
        final var variance = getEstimatedHardIronZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronZStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @return standard deviation of estimated magnetometer bias coordinates.
     */
    public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
        return estimatedCovariance != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                getEstimatedHardIronXStandardDeviation(),
                getEstimatedHardIronYStandardDeviation(),
                getEstimatedHardIronZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of magnetometer bias was available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedHardIronXStandardDeviation(),
                    getEstimatedHardIronYStandardDeviation(),
                    getEstimatedHardIronZStandardDeviation(),
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates
     * expressed in Teslas (T).
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public Double getEstimatedHardIronStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
                + getEstimatedHardIronZStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(),
                MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of magnetometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(
            final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationAverage());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias expressed in
     * Teslas (T).
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public Double getEstimatedHardIronStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedHardIronXVariance()
                + getEstimatedHardIronYVariance()
                + getEstimatedHardIronZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of magnetometer bias
     * is available, false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationNorm());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1049
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1019
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1048
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1022
this(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa);
        this.listener = listener;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2101
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2065
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2113
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2152
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 1052
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2103
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 1051
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2067
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX,
            final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz,
            final double accelerometerMyx, final double accelerometerMyz,
            final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy,
            final double accelerometerSz, final double accelerometerMxy,
            final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx,
            final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
                accelerometerMyz, accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1022
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1025
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 2115
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 2154
}

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasX() {
        return accelerometerBiasX;
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX known x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = accelerometerBiasX;
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasY() {
        return accelerometerBiasY;
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasY known y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = accelerometerBiasY;
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public double getAccelerometerBiasZ() {
        return accelerometerBiasZ;
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasZ known z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasXAsAcceleration() {
        return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasYAsAcceleration() {
        return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getAccelerometerBiasZAsAcceleration() {
        return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerometerBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z-coordinate of accelerometer bias to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = accelerometerBiasX;
        this.accelerometerBiasY = accelerometerBiasY;
        this.accelerometerBiasZ = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerBiasX x-coordinate of accelerometer bias.
     * @param accelerometerBiasY y-coordinate of accelerometer bias.
     * @param accelerometerBiasZ z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerBias(
            final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
            final Acceleration accelerometerBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
        this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
        this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public double[] getAccelerometerBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getAccelerometerBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getAccelerometerBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result[0] = accelerometerBiasX;
        result[1] = accelerometerBiasY;
        result[2] = accelerometerBiasZ;
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias[0];
        accelerometerBiasY = accelerometerBias[1];
        accelerometerBiasZ = accelerometerBias[2];
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return known accelerometer bias.
     */
    @Override
    public Matrix getAccelerometerBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getAccelerometerBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getAccelerometerBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerBiasX);
        result.setElementAtIndex(1, accelerometerBiasY);
        result.setElementAtIndex(2, accelerometerBiasZ);
    }

    /**
     * Sets known accelerometer bias to be used to fix measured specific
     * force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param accelerometerBias known accelerometer bias. Must be 3x1.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
        accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
        accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
    }

    /**
     * Gets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer x scaling factor.
     */
    @Override
    public double getAccelerometerSx() {
        return accelerometerSx;
    }

    /**
     * Sets known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
    }

    /**
     * Gets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer y scaling factor.
     */
    @Override
    public double getAccelerometerSy() {
        return accelerometerSy;
    }

    /**
     * Sets known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSy known accelerometer y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSy = accelerometerSy;
    }

    /**
     * Gets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @return known accelerometer z scaling factor.
     */
    @Override
    public double getAccelerometerSz() {
        return accelerometerSz;
    }

    /**
     * Sets known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     *
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Gets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-y cross coupling error.
     */
    @Override
    public double getAccelerometerMxy() {
        return accelerometerMxy;
    }

    /**
     * Sets known accelerometer x-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
    }

    /**
     * Gets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer x-z cross coupling error.
     */
    @Override
    public double getAccelerometerMxz() {
        return accelerometerMxz;
    }

    /**
     * Sets known accelerometer x-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxz known accelerometer x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxz = accelerometerMxz;
    }

    /**
     * Gets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-x cross coupling error.
     */
    @Override
    public double getAccelerometerMyx() {
        return accelerometerMyx;
    }

    /**
     * Sets known accelerometer y-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyx = accelerometerMyx;
    }

    /**
     * Gets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer y-z cross coupling error.
     */
    @Override
    public double getAccelerometerMyz() {
        return accelerometerMyz;
    }

    /**
     * Sets known accelerometer y-z cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMyz = accelerometerMyz;
    }

    /**
     * Gets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-x cross coupling error.
     */
    @Override
    public double getAccelerometerMzx() {
        return accelerometerMzx;
    }

    /**
     * Sets known accelerometer z-x cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzx = accelerometerMzx;
    }

    /**
     * Gets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @return known accelerometer z-y cross coupling error.
     */
    @Override
    public double getAccelerometerMzy() {
        return accelerometerMzy;
    }

    /**
     * Sets known accelerometer z-y cross coupling error to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors to be used to fix measured
     * specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerSx known accelerometer x scaling factor.
     * @param accelerometerSy known accelerometer y scaling factor.
     * @param accelerometerSz known accelerometer z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerSx = accelerometerSx;
        this.accelerometerSy = accelerometerSy;
        this.accelerometerSz = accelerometerSz;
    }

    /**
     * Sets known accelerometer cross coupling errors to be used to fix
     * measured specific force and find cross biases introduced by the
     * accelerometer.
     *
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerCrossCouplingErrors(
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.accelerometerMxy = accelerometerMxy;
        this.accelerometerMxz = accelerometerMxz;
        this.accelerometerMyx = accelerometerMyx;
        this.accelerometerMyz = accelerometerMyz;
        this.accelerometerMzx = accelerometerMzx;
        this.accelerometerMzy = accelerometerMzy;
    }

    /**
     * Sets known accelerometer scaling factors and cross coupling errors
     * to be used to fix measured specific force and find cross biases
     * introduced by the accelerometer.
     *
     * @param accelerometerSx  known accelerometer x scaling factor.
     * @param accelerometerSy  known accelerometer y scaling factor.
     * @param accelerometerSz  known accelerometer z scaling factor.
     * @param accelerometerMxy known accelerometer x-y cross coupling
     *                         error.
     * @param accelerometerMxz known accelerometer x-z cross coupling
     *                         error.
     * @param accelerometerMyx known accelerometer y-x cross coupling
     *                         error.
     * @param accelerometerMyz known accelerometer y-z cross coupling
     *                         error.
     * @param accelerometerMzx known accelerometer z-x cross coupling
     *                         error.
     * @param accelerometerMzy known accelerometer z-y cross coupling
     *                         error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
            final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
            final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
            final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
        setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx, accelerometerMyz,
                accelerometerMzx, accelerometerMzy);
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @return known accelerometer scale factors and cross coupling
     * errors matrix.
     */
    @Override
    public Matrix getAccelerometerMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getAccelerometerMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getAccelerometerMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, accelerometerSx);
        result.setElementAtIndex(1, accelerometerMyx);
        result.setElementAtIndex(2, accelerometerMzx);

        result.setElementAtIndex(3, accelerometerMxy);
        result.setElementAtIndex(4, accelerometerSy);
        result.setElementAtIndex(5, accelerometerMzy);

        result.setElementAtIndex(6, accelerometerMxz);
        result.setElementAtIndex(7, accelerometerMyz);
        result.setElementAtIndex(8, accelerometerSz);
    }

    /**
     * Sets known accelerometer scale factors and cross coupling
     * errors matrix.
     *
     * @param accelerometerMa known accelerometer scale factors and
     *                        cross coupling errors matrix. Must be 3x3.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
                || accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        accelerometerSx = accelerometerMa.getElementAtIndex(0);
        accelerometerMyx = accelerometerMa.getElementAtIndex(1);
        accelerometerMzx = accelerometerMa.getElementAtIndex(2);

        accelerometerMxy = accelerometerMa.getElementAtIndex(3);
        accelerometerSy = accelerometerMa.getElementAtIndex(4);
        accelerometerMzy = accelerometerMa.getElementAtIndex(5);

        accelerometerMxz = accelerometerMa.getElementAtIndex(6);
        accelerometerMyz = accelerometerMa.getElementAtIndex(7);
        accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2865
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4157
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2865
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3225
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4999
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2974
listener.onCalibrateEnd((C) this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFy() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFz() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @return estimated accelerometer bias or null if not available.
     */
    @Override
    public AccelerationTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null ?
                new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                        estimatedBiases[0], estimatedBiases[1], estimatedBiases[2]) : null;
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated accelerometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }


    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated x coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFxVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFxStandardDeviation() {
        final var variance = getEstimatedBiasFxVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFxStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated y coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFyVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFyStandardDeviation() {
        final var variance = getEstimatedBiasFyVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFyStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated z coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFzVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFzStandardDeviation() {
        final var variance = getEstimatedBiasFzVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFzStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @return standard deviation of estimated accelerometer bias coordinates.
     */
    public AccelerationTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null ?
                new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                        getEstimatedBiasFxStandardDeviation(),
                        getEstimatedBiasFyStandardDeviation(),
                        getEstimatedBiasFzStandardDeviation()) : null;
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of accelerometer bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasFxStandardDeviation(),
                    getEstimatedBiasFyStandardDeviation(),
                    getEstimatedBiasFzStandardDeviation(),
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates expressed
     * in meters per squared second (m/s^2).
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null ?
                (getEstimatedBiasFxStandardDeviation() + getEstimatedBiasFyStandardDeviation()
                        + getEstimatedBiasFzStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null.
     */
    public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
        return estimatedCovariance != null ?
                new Acceleration(getEstimatedBiasStandardDeviationAverage(),
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias coordinates.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of accelerometer bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3225
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4157
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null
                ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2141
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3717
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return estimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (estimatedHardIron != null) {
            System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedHardIron != null) {
            result.fromArray(estimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return estimatedHardIron != null ? estimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return estimatedHardIron != null ? estimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return estimatedHardIron != null ? estimatedHardIron[2] : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @return x coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[0]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @return y coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[1]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @return z coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[2]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @return estimated magnetometer bias or null if not available.
     */
    @Override
    public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
        return estimatedHardIron != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
                : null;
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
        if (estimatedHardIron != null) {
            result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated x coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronXStandardDeviation() {
        final var variance = getEstimatedHardIronXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(
            final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronXStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated y coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronYStandardDeviation() {
        final var variance = getEstimatedHardIronYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(
            final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronYStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated z coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronZStandardDeviation() {
        final var variance = getEstimatedHardIronZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronZStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @return standard deviation of estimated magnetometer bias coordinates.
     */
    public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
        return estimatedCovariance != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                getEstimatedHardIronXStandardDeviation(),
                getEstimatedHardIronYStandardDeviation(),
                getEstimatedHardIronZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of magnetometer bias was available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedHardIronXStandardDeviation(),
                    getEstimatedHardIronYStandardDeviation(),
                    getEstimatedHardIronZStandardDeviation(),
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates
     * expressed in Teslas (T).
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public Double getEstimatedHardIronStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
                + getEstimatedHardIronZStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of magnetometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationAverage());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias expressed in
     * Teslas (T).
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public Double getEstimatedHardIronStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedHardIronXVariance() + getEstimatedHardIronYVariance()
                + getEstimatedHardIronZVariance()) : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of magnetometer bias
     * is available, false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(
            final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationNorm());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1354
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1499
public void getEcefVelocity(final ECEFVelocity result) {
        result.setCoordinates(vx, vy, vz);
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @return estimated ECEF user velocity.
     */
    public ECEFVelocity getEcefVelocity() {
        return new ECEFVelocity(vx, vy, vz);
    }

    /**
     * Sets estimated ECEF user velocity.
     *
     * @param ecefVelocity estimated ECEF user velocity.
     */
    public void setEcefVelocity(final ECEFVelocity ecefVelocity) {
        vx = ecefVelocity.getVx();
        vy = ecefVelocity.getVy();
        vz = ecefVelocity.getVz();
    }

    /**
     * Gets x coordinate of estimated ECEF user position.
     *
     * @param result instance where x coordinate of estimated ECEF user position
     *               will be stored.
     */
    public void getDistanceX(final Distance result) {
        result.setValue(x);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets x coordinate of estimated ECEF user position.
     *
     * @return x coordinate of estimated ECEF user position.
     */
    public Distance getDistanceX() {
        return new Distance(x, DistanceUnit.METER);
    }

    /**
     * Sets x coordinate of estimated ECEF user position.
     *
     * @param x x coordinate of estimated ECEF user position.
     */
    public void setDistanceX(final Distance x) {
        this.x = DistanceConverter.convert(x.getValue().doubleValue(), x.getUnit(), DistanceUnit.METER);
    }

    /**
     * Gets y coordinate of estimated ECEF user position.
     *
     * @param result instance where y coordinate of estimated ECEF user position
     *               will be stored.
     */
    public void getDistanceY(final Distance result) {
        result.setValue(y);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets y coordinate of estimated ECEF user position.
     *
     * @return y coordinate of estimated ECEF user position.
     */
    public Distance getDistanceY() {
        return new Distance(y, DistanceUnit.METER);
    }

    /**
     * Sets y coordinate of estimated ECEF user position.
     *
     * @param y y coordinate of estimated ECEF user position.
     */
    public void setDistanceY(final Distance y) {
        this.y = DistanceConverter.convert(y.getValue().doubleValue(), y.getUnit(), DistanceUnit.METER);
    }

    /**
     * Gets z coordinate of estimated ECEF user position.
     *
     * @param result instance where z coordinate of estimated ECEF user position
     *               will be stored.
     */
    public void getDistanceZ(final Distance result) {
        result.setValue(z);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets z coordinate of estimated ECEF user position.
     *
     * @return z coordinate of estimated ECEF user position.
     */
    public Distance getDistanceZ() {
        return new Distance(z, DistanceUnit.METER);
    }

    /**
     * Sets z coordinate of estimated ECEF user position.
     *
     * @param z z coordinate of estimated ECEF user position.
     */
    public void setDistanceZ(final Distance z) {
        this.z = DistanceConverter.convert(z.getValue().doubleValue(), z.getUnit(), DistanceUnit.METER);
    }

    /**
     * Sets coordinates of estimated ECEF user position.
     *
     * @param x x coordinate of estimated ECEF user position.
     * @param y y coordinate of estimated ECEF user position.
     * @param z z coordinate of estimated ECEF user position.
     */
    public void setPositionCoordinates(final Distance x, final Distance y, final Distance z) {
        setDistanceX(x);
        setDistanceY(y);
        setDistanceZ(z);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @param result instance where estimated ECEF user position expressed
     *               in meters (m) will be stored.
     */
    public void getPosition(final Point3D result) {
        result.setInhomogeneousCoordinates(x, y, z);
    }

    /**
     * Gets estimated ECEF user position expressed in meters (m).
     *
     * @return estimated ECEF user position expressed in meters (m).
     */
    public Point3D getPosition() {
        return new InhomogeneousPoint3D(x, y, z);
    }

    /**
     * Sets estimated ECEF user position expressed in meters (m).
     *
     * @param position estimated ECEF user position expressed in
     *                 meters (m).
     */
    public void setPosition(final Point3D position) {
        x = position.getInhomX();
        y = position.getInhomY();
        z = position.getInhomZ();
    }

    /**
     * Gets estimated ECEF user position.
     *
     * @param result instance where estimated ECEF user position
     *               will be stored.
     */
    public void getEcefPosition(final ECEFPosition result) {
        result.setCoordinates(x, y, z);
    }

    /**
     * Gets estimated ECEF user position.
     *
     * @return estimated ECEF user position.
     */
    public ECEFPosition getEcefPosition() {
        return new ECEFPosition(x, y, z);
    }

    /**
     * Sets estimated ECEF user position.
     *
     * @param ecefPosition estimated ECEF user position.
     */
    public void setEcefPosition(final ECEFPosition ecefPosition) {
        x = ecefPosition.getX();
        y = ecefPosition.getY();
        z = ecefPosition.getZ();
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @param result instance where estimated ECEF user position and velocity
     *               will be stored.
     */
    public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
        result.setPositionCoordinates(x, y, z);
        result.setVelocityCoordinates(vx, vy, vz);
    }

    /**
     * Gets estimated ECEF user position and velocity.
     *
     * @return estimated ECEF user position and velocity.
     */
    public ECEFPositionAndVelocity getPositionAndVelocity() {
        return new ECEFPositionAndVelocity(x, y, z, vx, vy, vz);
    }

    /**
     * Sets estimated ECEF user position and velocity.
     *
     * @param positionAndVelocity estimated ECEF user position and velocity.
     */
    public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {
        x = positionAndVelocity.getX();
        y = positionAndVelocity.getY();
        z = positionAndVelocity.getZ();
        vx = positionAndVelocity.getVx();
        vy = positionAndVelocity.getVy();
        vz = positionAndVelocity.getVz();
    }

    /**
     * Gets body to ECEF frame containing coordinate transformation, position and
     * velocity.
     *
     * @param result instance where body to ECEF frame will be stored.
     * @return true if result was updated, false otherwise.
     */
    public boolean getFrame(final ECEFFrame result) {
        if (bodyToEcefCoordinateTransformationMatrix != null) {
            try {
                result.setCoordinateTransformation(getC());
            } catch (final InvalidSourceAndDestinationFrameTypeException | InvalidRotationMatrixException e) {
                return false;
            }
            result.setCoordinates(x, y, z);
            result.setVelocityCoordinates(vx, vy, vz);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets body to ECEF frame containing coordinate transformation, position and
     * velocity.
     *
     * @return body to ECEF frame.
     */
    public ECEFFrame getFrame() {
        if (bodyToEcefCoordinateTransformationMatrix != null) {
            try {
                return new ECEFFrame(x, y, z, vx, vy, vz, getC());
            } catch (final InvalidSourceAndDestinationFrameTypeException | InvalidRotationMatrixException e) {
                return null;
            }
        } else {
            return null;
        }
    }

    /**
     * Sets body to ECEF frame containing coordinate transformation, position and
     * velocity.
     *
     * @param frame body to ECEF frame to be set.
     */
    public void setFrame(final ECEFFrame frame) {
        x = frame.getX();
        y = frame.getY();
        z = frame.getZ();

        vx = frame.getVx();
        vy = frame.getVy();
        vz = frame.getVz();

        if (bodyToEcefCoordinateTransformationMatrix != null) {
            frame.getCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
        } else {
            bodyToEcefCoordinateTransformationMatrix = frame.getCoordinateTransformationMatrix();
        }
    }

    /**
     * Gets estimated accelerometer bias resolved around x axis.
     *
     * @param result instance where estimated accelerometer bias resolved around
     *               x axis will be stored.
     */
    public void getAccelerationBiasXAsAcceleration(final Acceleration result) {
        result.setValue(accelerationBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around x axis.
     *
     * @return estimated accelerometer bias resolved around x axis.
     */
    public Acceleration getAccelerationBiasXAsAcceleration() {
        return new Acceleration(accelerationBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias resolved around x axis.
     *
     * @param accelerationBiasX estimated accelerometer bias resolved
     *                          around x axis.
     */
    public void setAccelerationBiasX(final Acceleration accelerationBiasX) {
        this.accelerationBiasX = AccelerationConverter.convert(accelerationBiasX.getValue().doubleValue(),
                accelerationBiasX.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around y axis.
     *
     * @param result instance where estimated accelerometer bias resolved around
     *               y axis will be stored.
     */
    public void getAccelerationBiasYAsAcceleration(final Acceleration result) {
        result.setValue(accelerationBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around y axis.
     *
     * @return estimated accelerometer bias resolved around y axis.
     */
    public Acceleration getAccelerationBiasYAsAcceleration() {
        return new Acceleration(accelerationBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias resolved around y axis.
     *
     * @param accelerationBiasY estimated accelerometer bias resolved
     *                          around y axis.
     */
    public void setAccelerationBiasY(final Acceleration accelerationBiasY) {
        this.accelerationBiasY = AccelerationConverter.convert(accelerationBiasY.getValue().doubleValue(),
                accelerationBiasY.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around z axis.
     *
     * @param result instance where estimated accelerometer bias resolved around
     *               z axis will be stored.
     */
    public void getAccelerationBiasZAsAcceleration(final Acceleration result) {
        result.setValue(accelerationBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets estimated accelerometer bias resolved around z axis.
     *
     * @return estimated accelerometer bias resolved around z axis.
     */
    public Acceleration getAccelerationBiasZAsAcceleration() {
        return new Acceleration(accelerationBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias resolved around z axis.
     *
     * @param accelerationBiasZ estimated accelerometer bias resolved
     *                          around z axis.
     */
    public void setAccelerationBiasZ(final Acceleration accelerationBiasZ) {
        this.accelerationBiasZ = AccelerationConverter.convert(accelerationBiasZ.getValue().doubleValue(),
                accelerationBiasZ.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets estimated accelerometer bias coordinates.
     *
     * @param accelerationBiasX estimated accelerometer bias resolved around x axis.
     * @param accelerationBiasY estimated accelerometer bias resolved around y axis.
     * @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
     */
    public void setAccelerationBiasCoordinates(
            final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
            final Acceleration accelerationBiasZ) {
        setAccelerationBiasX(accelerationBiasX);
        setAccelerationBiasY(accelerationBiasY);
        setAccelerationBiasZ(accelerationBiasZ);
    }

    /**
     * Gets estimated gyroscope bias resolved around x axis.
     *
     * @param result instance where estimated gyroscope bias resolved around x axis will
     *               be stored.
     */
    public void getAngularSpeedGyroBiasX(final AngularSpeed result) {
        result.setValue(gyroBiasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around x axis.
     *
     * @return estimated gyroscope bias resolved around x axis.
     */
    public AngularSpeed getAngularSpeedGyroBiasX() {
        return new AngularSpeed(gyroBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias resolved around x axis.
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis.
     */
    public void setGyroBiasX(final AngularSpeed gyroBiasX) {
        this.gyroBiasX = AngularSpeedConverter.convert(gyroBiasX.getValue().doubleValue(), gyroBiasX.getUnit(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around y axis.
     *
     * @param result instance where estimated gyroscope bias resolved around y axis will
     *               be stored.
     */
    public void getAngularSpeedGyroBiasY(final AngularSpeed result) {
        result.setValue(gyroBiasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around y axis.
     *
     * @return estimated gyroscope bias resolved around y axis.
     */
    public AngularSpeed getAngularSpeedGyroBiasY() {
        return new AngularSpeed(gyroBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias resolved around y axis.
     *
     * @param gyroBiasY estimated gyroscope bias resolved around y axis.
     */
    public void setGyroBiasY(final AngularSpeed gyroBiasY) {
        this.gyroBiasY = AngularSpeedConverter.convert(gyroBiasY.getValue().doubleValue(), gyroBiasY.getUnit(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around z axis.
     *
     * @param result instance where estimated gyroscope bias resolved around z axis will
     *               be stored.
     */
    public void getAngularSpeedGyroBiasZ(final AngularSpeed result) {
        result.setValue(gyroBiasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets estimated gyroscope bias resolved around z axis.
     *
     * @return estimated gyroscope bias resolved around z axis.
     */
    public AngularSpeed getAngularSpeedGyroBiasZ() {
        return new AngularSpeed(gyroBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias resolved around z axis.
     *
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
     */
    public void setGyroBiasZ(final AngularSpeed gyroBiasZ) {
        this.gyroBiasZ = AngularSpeedConverter.convert(gyroBiasZ.getValue().doubleValue(), gyroBiasZ.getUnit(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets estimated gyroscope bias coordinates.
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis.
     * @param gyroBiasY estimated gyroscope bias resolved around y axis.
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis.
     */
    public void setGyroBiasCoordinates(
            final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY, final AngularSpeed gyroBiasZ) {
        setGyroBiasX(gyroBiasX);
        setGyroBiasY(gyroBiasY);
        setGyroBiasZ(gyroBiasZ);
    }

    /**
     * Copies this instance data into provided instance.
     *
     * @param output destination instance where data will be copied to.
     */
    public void copyTo(final INSLooselyCoupledKalmanState output) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4344
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2300
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 741
initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as a column matrix with
     * values expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4344
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2300
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1409
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1365
initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as a column matrix with
     * values expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3433
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3471
final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix with values expressed in radians per second (rad/s).
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @return initial bias coordinates.
     */
    public AngularSpeedTriad getInitialBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(final AngularSpeedTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return turntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        this.turntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
        result.setValue(turntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public List<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 711
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2651
public void setListener(final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known x coordinate of gyroscope bias.
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known y coordinate of gyroscope bias.
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known z coordinate of gyroscope bias.
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known gyroscope bias coordinates expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @param biasY y coordinate of gyroscope bias.
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known gyroscope bias coordinates.
     *
     * @param biasX x coordinate of gyroscope bias.
     * @param biasY y coordinate of gyroscope bias.
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAngularSpeed(biasX);
        this.biasY = convertAngularSpeed(biasY);
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinate of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known gyroscope bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     *
     * @return known gyroscope bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known gyroscope bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known gyroscope bias as a column matrix.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 382
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 398
public void setBias(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
        setBiasX(biasX);
        setBiasY(biasY);
        setBiasZ(biasZ);
    }

    /**
     * Gets cross coupling errors matrix.
     *
     * @return cross coupling errors matrix.
     */
    public Matrix getCrossCouplingErrors() {
        return new Matrix(crossCouplingErrors);
    }

    /**
     * Gets cross coupling errors matrix.
     *
     * @param result instance where result will be stored.
     */
    public void getCrossCouplingErrors(final Matrix result) {
        crossCouplingErrors.copyTo(result);
    }

    /**
     * Sets cross coupling errors matrix.
     *
     * @param crossCouplingErrors cross coupling errors matrix. Must be 3x3.
     * @throws AlgebraException         if provided matrix cannot be inverted.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException {
        if (crossCouplingErrors.getRows() != BodyKinematics.COMPONENTS
                || crossCouplingErrors.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        this.crossCouplingErrors = crossCouplingErrors;

        identity.add(crossCouplingErrors, tmp1);

        Utils.inverse(tmp1, tmp2);
    }

    /**
     * Gets x scaling factor.
     *
     * @return x scaling factor.
     */
    public double getSx() {
        return crossCouplingErrors.getElementAt(0, 0);
    }

    /**
     * Sets x scaling factor
     *
     * @param sx x scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setSx(final double sx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y scaling factor.
     *
     * @return y scaling factor.
     */
    public double getSy() {
        return crossCouplingErrors.getElementAt(1, 1);
    }

    /**
     * Sets y scaling factor.
     *
     * @param sy y scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setSy(final double sy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 1, sy);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z scaling factor.
     *
     * @return z scaling factor.
     */
    public double getSz() {
        return crossCouplingErrors.getElementAt(2, 2);
    }

    /**
     * Sets z scaling factor.
     *
     * @param sz z scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setSz(final double sz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 2, sz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets x-y cross coupling error.
     *
     * @return x-y cross coupling error.
     */
    public double getMxy() {
        return crossCouplingErrors.getElementAt(0, 1);
    }

    /**
     * Sets x-y cross coupling error.
     *
     * @param mxy x-y cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMxy(final double mxy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 1, mxy);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets x-z cross coupling error.
     *
     * @return x-z cross coupling error.
     */
    public double getMxz() {
        return crossCouplingErrors.getElementAt(0, 2);
    }

    /**
     * Sets x-z cross coupling error.
     *
     * @param mxz x-z cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMxz(final double mxz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 2, mxz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y-x cross coupling error.
     *
     * @return y-x cross coupling error.
     */
    public double getMyx() {
        return crossCouplingErrors.getElementAt(1, 0);
    }

    /**
     * Sets y-x cross coupling error.
     *
     * @param myx y-x cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMyx(final double myx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 0, myx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y-z cross coupling error.
     *
     * @return y-z cross coupling error.
     */
    public double getMyz() {
        return crossCouplingErrors.getElementAt(1, 2);
    }

    /**
     * Sets y-z cross coupling error.
     *
     * @param myz y-z cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMyz(final double myz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 2, myz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z-x cross coupling error.
     *
     * @return z-x cross coupling error.
     */
    public double getMzx() {
        return crossCouplingErrors.getElementAt(2, 0);
    }

    /**
     * Sets z-x cross coupling error.
     *
     * @param mzx z-x cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMzx(final double mzx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 0, mzx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z-y cross coupling error.
     *
     * @return z-y cross coupling error.
     */
    public double getMzy() {
        return crossCouplingErrors.getElementAt(2, 1);
    }

    /**
     * Sets z-y cross coupling error.
     *
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMzy(final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets scaling factors.
     *
     * @param sx x scaling factor.
     * @param sy y scaling factor.
     * @param sz z scaling factor.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non invertible.
     */
    public void setScalingFactors(final double sx, final double sy, final double sz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        m.setElementAt(1, 1, sy);
        m.setElementAt(2, 2, sz);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets cross coupling errors.
     *
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non invertible.
     */
    public void setCrossCouplingErrors(
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 1, mxy);
        m.setElementAt(0, 2, mxz);
        m.setElementAt(1, 0, myx);
        m.setElementAt(1, 2, myz);
        m.setElementAt(2, 0, mzx);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets scaling factors and cross coupling errors.
     *
     * @param sx  x scaling factor.
     * @param sy  y scaling factor.
     * @param sz  z scaling factor.
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non invertible.
     */
    public void setScalingFactorsAndCrossCouplingErrors(
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        m.setElementAt(1, 1, sy);
        m.setElementAt(2, 2, sz);
        m.setElementAt(0, 1, mxy);
        m.setElementAt(0, 2, mxz);
        m.setElementAt(1, 0, myx);
        m.setElementAt(1, 2, myz);
        m.setElementAt(2, 0, mzx);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Fixes provided measured specific force values by undoing the errors
     * introduced by the accelerometer model to restore the true specific
     * force.
     * This method uses last provided bias and cross coupling errors.
     *
     * @param measuredF measured specific force.
     * @param result    instance where restored true specific force will be stored.
     *                  Mut have length 3.
     * @throws AlgebraException         if there are numerical instabilities.
     * @throws IllegalArgumentException if length of provided result array is
     *                                  not 3.
     */
    public void fix(final AccelerationTriad measuredF, final double[] result) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3403
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4730
setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
        final var m21 = result[1];
        final var m31 = result[2];

        final var m12 = result[3];
        final var m22 = result[4];
        final var m32 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var g11 = result[9];
        final var g21 = result[10];
        final var g31 = result[11];

        final var g12 = result[12];
        final var g22 = result[13];
        final var g32 = result[14];

        final var g13 = result[15];
        final var g23 = result[16];
        final var g33 = result[17];

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, m21);
        mm.setElementAtIndex(2, m31);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, m32);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mg);

        // at this point covariance is expressed in terms of M and G, and must
        // be expressed in terms of Mg and Gg.
        // We know that:
        // Mg = M - I
        // Gg = M * G

        // M = [m11  m12  m13]
        //     [m21  m22  m23]
        //     [m31  m32  m33]

        // G = [g11  g12  g13]
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [m21        m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [m31        m32         m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [m21  m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [m31  m32  m33][g31  g32  g33]

        // Defining the linear application:
        // F(M, G) = F(m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
        // as:
        // [sx] =   [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [m21]
        // [myz]    [m23]
        // [mzx]    [m31]
        // [mzy]    [m32]
        // [gg11]   [m11 * g11 + m12 * g21 + m13 * g31]
        // [gg21]   [m21 * g11 + m22 * g21 + m23 * g31]
        // [gg31]   [m31 * g11 + m32 * g21 + m33 * g31]
        // [gg12]   [m11 * g12 + m12 * g22 + m13 * g32]
        // [gg22]   [m21 * g12 + m22 * g22 + n23 * g32]
        // [gg32]   [m31 * g12 + m32 * g22 + m33 * g32]
        // [gg13]   [m11 * g13 + m12 * g23 + m13 * g33]
        // [gg23]   [m21 * g13 + m22 * g23 + m23 * g33]
        // [gg33]   [m31 * g13 + m32 * g23 + m33 * g33]

        // Then the Jacobian of F(M, G) is:
        // J = [1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [g11  0    0    g21  0    0    g31  0    0    m11  m12  m13  0    0    0    0    0    0  ]
        //     [0    g11  0    0    g21  0    0    g31  0    m21  m22  m23  0    0    0    0    0    0  ]
        //     [0    0    g11  0    0    g21  0    0    g31  m31  m32  m33  0    0    0    0    0    0  ]
        //     [g12  0    0    g22  0    0    g32  0    0    0    0    0    m11  m12  m13  0    0    0  ]
        //     [0    g12  0    0    g22  0    0    g32  0    0    0    0    m21  m22  m23  0    0    0  ]
        //     [0    0    g12  0    0    g22  0    0    g32  0    0    0    m31  m32  m33  0    0    0  ]
        //     [g13  0    0    g23  0    0    g33  0    0    0    0    0    0    0    0    m11  m12  m13]
        //     [0    g13  0    0    g23  0    0    g33  0    0    0    0    0    0    0    m21  m22  m23]
        //     [0    0    g13  0    0    g23  0    0    g33  0    0    0    0    0    0    m31  m32  m33]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS_AND_CROSS_BIASES);

        jacobian.setElementAt(0, 0, 1.0);
        jacobian.setElementAt(1, 4, 1.0);
        jacobian.setElementAt(2, 8, 1.0);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 6, 1.0);
        jacobian.setElementAt(5, 1, 1.0);

        jacobian.setElementAt(6, 7, 1.0);
        jacobian.setElementAt(7, 2, 1.0);
        jacobian.setElementAt(8, 5, 1.9);

        jacobian.setElementAt(9, 0, g11);
        jacobian.setElementAt(9, 3, g21);
        jacobian.setElementAt(9, 6, g31);
        jacobian.setElementAt(9, 9, m11);
        jacobian.setElementAt(9, 10, m12);
        jacobian.setElementAt(9, 11, m13);

        jacobian.setElementAt(10, 1, g11);
        jacobian.setElementAt(10, 4, g21);
        jacobian.setElementAt(10, 7, g31);
        jacobian.setElementAt(10, 9, m21);
        jacobian.setElementAt(10, 10, m22);
        jacobian.setElementAt(10, 11, m23);

        jacobian.setElementAt(11, 2, g11);
        jacobian.setElementAt(11, 5, g21);
        jacobian.setElementAt(11, 8, g31);
        jacobian.setElementAt(11, 9, m31);
        jacobian.setElementAt(11, 10, m32);
        jacobian.setElementAt(11, 11, m33);

        jacobian.setElementAt(12, 0, g12);
        jacobian.setElementAt(12, 3, g22);
        jacobian.setElementAt(12, 6, g32);
        jacobian.setElementAt(12, 12, m11);
        jacobian.setElementAt(12, 13, m12);
        jacobian.setElementAt(12, 14, m13);

        jacobian.setElementAt(13, 1, g12);
        jacobian.setElementAt(13, 4, g22);
        jacobian.setElementAt(13, 7, g32);
        jacobian.setElementAt(13, 12, m21);
        jacobian.setElementAt(13, 13, m22);
        jacobian.setElementAt(13, 14, m23);

        jacobian.setElementAt(14, 2, g12);
        jacobian.setElementAt(14, 5, g22);
        jacobian.setElementAt(14, 8, g32);
        jacobian.setElementAt(14, 12, m31);
        jacobian.setElementAt(14, 13, m32);
        jacobian.setElementAt(14, 14, m33);

        jacobian.setElementAt(15, 0, g13);
        jacobian.setElementAt(15, 3, g23);
        jacobian.setElementAt(15, 6, g33);
        jacobian.setElementAt(15, 15, m11);
        jacobian.setElementAt(15, 16, m12);
        jacobian.setElementAt(15, 17, m13);

        jacobian.setElementAt(16, 1, g13);
        jacobian.setElementAt(16, 4, g23);
        jacobian.setElementAt(16, 7, g33);
        jacobian.setElementAt(16, 15, m21);
        jacobian.setElementAt(16, 16, m22);
        jacobian.setElementAt(16, 17, m23);

        jacobian.setElementAt(17, 2, g13);
        jacobian.setElementAt(17, 5, g23);
        jacobian.setElementAt(17, 8, g33);
        jacobian.setElementAt(17, 15, m31);
        jacobian.setElementAt(17, 16, m32);
        jacobian.setElementAt(17, 17, m33);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and G-dependent cross biases
     * are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2793
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4054
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2804
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1514
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2114
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1383
}

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz,
            final double initialMyx, final double initialMyz,
            final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3556
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3603
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return turntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        this.turntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
        result.setValue(turntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements collection of body kinematics measurements at a
     *                     known position with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<StandardDeviationBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final var result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {
        if (position != null) {
            final var velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    position.getX(), position.getY(), position.getZ(), 0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = convertPosition(position);
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_BODY_KINEMATICS_MEASUREMENT;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return estimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public KnownBiasTurntableGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3515
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3567
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return turntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        this.turntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
        result.setValue(turntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public List<StandardDeviationBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements collection of body kinematics measurements at a
     *                     known position with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final var result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (position != null) {
            final var velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    position.getX(), position.getY(), position.getZ(), 0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = convertPosition(position);
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_BODY_KINEMATICS_MEASUREMENT;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return estimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasTurntableGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 415
com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java 431
|| crossCouplingErrors.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        this.crossCouplingErrors = crossCouplingErrors;

        identity.add(crossCouplingErrors, tmp1);

        Utils.inverse(tmp1, tmp2);
    }

    /**
     * Gets x scaling factor.
     *
     * @return x scaling factor.
     */
    public double getSx() {
        return crossCouplingErrors.getElementAt(0, 0);
    }

    /**
     * Sets x scaling factor
     *
     * @param sx x scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setSx(final double sx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y scaling factor.
     *
     * @return y scaling factor.
     */
    public double getSy() {
        return crossCouplingErrors.getElementAt(1, 1);
    }

    /**
     * Sets y scaling factor.
     *
     * @param sy y scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setSy(final double sy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 1, sy);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z scaling factor.
     *
     * @return z scaling factor.
     */
    public double getSz() {
        return crossCouplingErrors.getElementAt(2, 2);
    }

    /**
     * Sets z scaling factor.
     *
     * @param sz z scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setSz(final double sz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 2, sz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets x-y cross coupling error.
     *
     * @return x-y cross coupling error.
     */
    public double getMxy() {
        return crossCouplingErrors.getElementAt(0, 1);
    }

    /**
     * Sets x-y cross coupling error.
     *
     * @param mxy x-y cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMxy(final double mxy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 1, mxy);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets x-z cross coupling error.
     *
     * @return x-z cross coupling error.
     */
    public double getMxz() {
        return crossCouplingErrors.getElementAt(0, 2);
    }

    /**
     * Sets x-z cross coupling error.
     *
     * @param mxz x-z cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMxz(final double mxz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 2, mxz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y-x cross coupling error.
     *
     * @return y-x cross coupling error.
     */
    public double getMyx() {
        return crossCouplingErrors.getElementAt(1, 0);
    }

    /**
     * Sets y-x cross coupling error.
     *
     * @param myx y-x cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMyx(final double myx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 0, myx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y-z cross coupling error.
     *
     * @return y-z cross coupling error.
     */
    public double getMyz() {
        return crossCouplingErrors.getElementAt(1, 2);
    }

    /**
     * Sets y-z cross coupling error.
     *
     * @param myz y-z cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMyz(final double myz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 2, myz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z-x cross coupling error.
     *
     * @return z-x cross coupling error.
     */
    public double getMzx() {
        return crossCouplingErrors.getElementAt(2, 0);
    }

    /**
     * Sets z-x cross coupling error.
     *
     * @param mzx z-x cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMzx(final double mzx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 0, mzx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z-y cross coupling error.
     *
     * @return z-y cross coupling error.
     */
    public double getMzy() {
        return crossCouplingErrors.getElementAt(2, 1);
    }

    /**
     * Sets z-y cross coupling error.
     *
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non invertible.
     */
    public void setMzy(final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets scaling factors.
     *
     * @param sx x scaling factor.
     * @param sy y scaling factor.
     * @param sz z scaling factor.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non invertible.
     */
    public void setScalingFactors(final double sx, final double sy, final double sz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        m.setElementAt(1, 1, sy);
        m.setElementAt(2, 2, sz);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets cross coupling errors.
     *
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non invertible.
     */
    public void setCrossCouplingErrors(
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 1, mxy);
        m.setElementAt(0, 2, mxz);
        m.setElementAt(1, 0, myx);
        m.setElementAt(1, 2, myz);
        m.setElementAt(2, 0, mzx);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets scaling factors and cross coupling errors.
     *
     * @param sx  x scaling factor.
     * @param sy  y scaling factor.
     * @param sz  z scaling factor.
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non invertible.
     */
    public void setScalingFactorsAndCrossCouplingErrors(
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        m.setElementAt(1, 1, sy);
        m.setElementAt(2, 2, sz);
        m.setElementAt(0, 1, mxy);
        m.setElementAt(0, 2, mxz);
        m.setElementAt(1, 0, myx);
        m.setElementAt(1, 2, myz);
        m.setElementAt(2, 0, mzx);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Fixes provided measured specific force values by undoing the errors
     * introduced by the accelerometer model to restore the true specific
     * force.
     * This method uses last provided bias and cross coupling errors.
     *
     * @param measuredF measured specific force.
     * @param result    instance where restored true specific force will be stored.
     *                  Mut have length 3.
     * @throws AlgebraException         if there are numerical instabilities.
     * @throws IllegalArgumentException if length of provided result array is
     *                                  not 3.
     */
    public void fix(final AccelerationTriad measuredF, final double[] result) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 431
com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java 431
|| crossCouplingErrors.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        this.crossCouplingErrors = crossCouplingErrors;

        identity.add(crossCouplingErrors, tmp1);

        Utils.inverse(tmp1, tmp2);
    }

    /**
     * Gets x scaling factor.
     *
     * @return x scaling factor.
     */
    public double getSx() {
        return crossCouplingErrors.getElementAt(0, 0);
    }

    /**
     * Sets x scaling factor
     *
     * @param sx x scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setSx(final double sx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y scaling factor.
     *
     * @return y scaling factor.
     */
    public double getSy() {
        return crossCouplingErrors.getElementAt(1, 1);
    }

    /**
     * Sets y scaling factor.
     *
     * @param sy y scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setSy(final double sy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 1, sy);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z scaling factor.
     *
     * @return z scaling factor.
     */
    public double getSz() {
        return crossCouplingErrors.getElementAt(2, 2);
    }

    /**
     * Sets z scaling factor.
     *
     * @param sz z scaling factor.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setSz(final double sz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 2, sz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets x-y cross coupling error.
     *
     * @return x-y cross coupling error.
     */
    public double getMxy() {
        return crossCouplingErrors.getElementAt(0, 1);
    }

    /**
     * Sets x-y cross coupling error.
     *
     * @param mxy x-y cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setMxy(final double mxy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 1, mxy);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets x-z cross coupling error.
     *
     * @return x-z cross coupling error.
     */
    public double getMxz() {
        return crossCouplingErrors.getElementAt(0, 2);
    }

    /**
     * Sets x-z cross coupling error.
     *
     * @param mxz x-z cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setMxz(final double mxz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 2, mxz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y-x cross coupling error.
     *
     * @return y-x cross coupling error.
     */
    public double getMyx() {
        return crossCouplingErrors.getElementAt(1, 0);
    }

    /**
     * Sets y-x cross coupling error.
     *
     * @param myx y-x cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setMyx(final double myx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 0, myx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets y-z cross coupling error.
     *
     * @return y-z cross coupling error.
     */
    public double getMyz() {
        return crossCouplingErrors.getElementAt(1, 2);
    }

    /**
     * Sets y-z cross coupling error.
     *
     * @param myz y-z cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setMyz(final double myz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(1, 2, myz);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z-x cross coupling error.
     *
     * @return z-x cross coupling error.
     */
    public double getMzx() {
        return crossCouplingErrors.getElementAt(2, 0);
    }

    /**
     * Sets z-x cross coupling error.
     *
     * @param mzx z-x cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setMzx(final double mzx) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 0, mzx);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets z-y cross coupling error.
     *
     * @return z-y cross coupling error.
     */
    public double getMzy() {
        return crossCouplingErrors.getElementAt(2, 1);
    }

    /**
     * Sets z-y cross coupling error.
     *
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided value makes cross coupling matrix
     *                          non-invertible.
     */
    public void setMzy(final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets scaling factors.
     *
     * @param sx x scaling factor.
     * @param sy y scaling factor.
     * @param sz z scaling factor.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non-invertible.
     */
    public void setScalingFactors(
            final double sx, final double sy, final double sz) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        m.setElementAt(1, 1, sy);
        m.setElementAt(2, 2, sz);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets cross coupling errors.
     *
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non-invertible.
     */
    public void setCrossCouplingErrors(
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 1, mxy);
        m.setElementAt(0, 2, mxz);
        m.setElementAt(1, 0, myx);
        m.setElementAt(1, 2, myz);
        m.setElementAt(2, 0, mzx);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Sets scaling factors and cross coupling errors.
     *
     * @param sx  x scaling factor.
     * @param sy  y scaling factor.
     * @param sz  z scaling factor.
     * @param mxy x-y cross coupling error.
     * @param mxz x-z cross coupling error.
     * @param myx y-x cross coupling error.
     * @param myz y-z cross coupling error.
     * @param mzx z-x cross coupling error.
     * @param mzy z-y cross coupling error.
     * @throws AlgebraException if provided values make cross coupling matrix
     *                          non-invertible.
     */
    public void setScalingFactorsAndCrossCouplingErrors(
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy) throws AlgebraException {
        final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
        m.copyFrom(crossCouplingErrors);
        m.setElementAt(0, 0, sx);
        m.setElementAt(1, 1, sy);
        m.setElementAt(2, 2, sz);
        m.setElementAt(0, 1, mxy);
        m.setElementAt(0, 2, mxz);
        m.setElementAt(1, 0, myx);
        m.setElementAt(1, 2, myz);
        m.setElementAt(2, 0, mzx);
        m.setElementAt(2, 1, mzy);
        setCrossCouplingErrors(m);
    }

    /**
     * Gets g-dependant cross biases matrix.
     *
     * @return g-dependant cross biases matrix.
     */
    public Matrix getGDependantCrossBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4015
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1375
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1331
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2075
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3120
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2371
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2368
final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @return initial bias coordinates.
     */
    public AngularSpeedTriad getInitialBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialBiasAsTriad(AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return sequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setSequences(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.sequences = sequences;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4346
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2302
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 743
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1411
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1367
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2464
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 728
initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of initial bias.
     */
    @Override
    public double[] getInitialBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial bias to be used to find a solution as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @return initial bias to be used to find a solution as a column matrix.
     */
    @Override
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial bias to be used to find a solution as a column matrix.
     * Values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial bias to be used to find a solution as a column matrix with
     * values expressed in meters per squared second (m/s^2).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4028
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5378
setResult(m, b, g);

        // at this point covariance is expressed in terms of b, M and G, and must
        // be expressed in terms of bg, Mg and Gg.
        // We know that:
        // bg = M * b
        // Mg = M - I
        // Gg = M * G

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11  m12  m13]
        //     [m21  m22  m23]
        //     [m31  m32  m33]

        // G = [g11  g12  g13]
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // bg = [m11  m12  m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
        //      [m21  m22  m23][by]   [m21 * bx + m22 * by + m23 * bz]   [bgy]
        //      [m31  m32  m33][bz]   [m31 * bx + m32 * by + m33 * bz]   [bgz]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx	sy	 myz]   [m21        m22 - 1     m23     ]
        //      [mzx	mzy  sz ]   [m31        m32         m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [m21  m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [m31  m32  m33][g31  g32  g33]

        // Defining the linear application:
        // F(b, M, G) = F(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
        // as:
        // [bgx] =  [m11 * bx + m12 * by + m13 * bz]
        // [bgy]    [m21 * bx + m22 * by + m23 * bz]
        // [bgz]    [m31 * bx + m32 * by + m33 * bz]
        // [sx]     [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [m21]
        // [myz]    [m23]
        // [mzx]    [m31]
        // [mzy]    [m32]
        // [gg11]   [m11 * g11 + m12 * g21 + m13 * g31]
        // [gg21]   [m21 * g11 + m22 * g21 + m23 * g31]
        // [gg31]   [m31 * g11 + m32 * g21 + m33 * g31]
        // [gg12]   [m11 * g12 + m12 * g22 + m13 * g32]
        // [gg22]   [m21 * g12 + m22 * g22 + m23 * g32]
        // [gg32]   [m31 * g12 + m32 * g22 + m33 * g32]
        // [gg13]   [m11 * g13 + m12 * g23 + m13 * g33]
        // [gg23]   [m21 * g13 + m22 * g23 + m23 * g33]
        // [gg33]   [m31 * g13 + m32 * g23 + m33 * g33]

        // Then the Jacobian of F(b, M, G) is:
        // J = [m11  m12  m13  bx   0    0    by   0    0    bz   0    0    0    0    0    0    0    0    0    0    0  ]
        //     [m21  m22  m23  0    bx   0    0    by   0    0    bz   0    0    0    0    0    0    0    0    0    0  ]
        //     [m31  m32  m33  0    0    bx   0    0    by   0    0    bz   0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    g11  0    0    g21  0    0    g31  0    0    m11  m12  m13  0    0    0    0    0    0  ]
        //     [0    0    0    0    g11  0    0    g21  0    0    g31  0    m21  m22  m23  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    g11  0    0    g21  0    0    g31  m31  m32  m33  0    0    0    0    0    0  ]
        //     [0    0    0    g12  0    0    g22  0    0    g32  0    0    0    0    0    m11  m12  m13  0    0    0  ]
        //     [0    0    0    0    g12  0    0    g22  0    0    g32  0    0    0    0    m21  m22  m23  0    0    0  ]
        //     [0    0    0    0    0    g12  0    0    g22  0    0    g32  0    0    0    m31  m32  m33  0    0    0  ]
        //     [0    0    0    g13  0    0    g23  0    0    g33  0    0    0    0    0    0    0    0    m11  m12  m13]
        //     [0    0    0    0    g13  0    0    g23  0    0    g33  0    0    0    0    0    0    0    m21  m22  m23]
        //     [0    0    0    0    0    g13  0    0    g23  0    0    g33  0    0    0    0    0    0    m31  m32  m33]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS_AND_CROSS_BIASES);

        jacobian.setElementAt(0, 0, m11);
        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(0, 6, by);
        jacobian.setElementAt(0, 9, bz);

        jacobian.setElementAt(1, 0, m21);
        jacobian.setElementAt(1, 1, m22);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(1, 4, bx);
        jacobian.setElementAt(1, 7, by);
        jacobian.setElementAt(1, 10, bz);

        jacobian.setElementAt(2, 0, m31);
        jacobian.setElementAt(2, 1, m32);
        jacobian.setElementAt(2, 2, m33);
        jacobian.setElementAt(2, 5, bx);
        jacobian.setElementAt(2, 8, by);
        jacobian.setElementAt(2, 11, bz);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 7, 1.0);
        jacobian.setElementAt(5, 11, 1.0);

        jacobian.setElementAt(6, 6, 1.0);
        jacobian.setElementAt(7, 9, 1.0);
        jacobian.setElementAt(8, 4, 1.0);

        jacobian.setElementAt(9, 10, 1.0);
        jacobian.setElementAt(10, 5, 1.0);
        jacobian.setElementAt(11, 8, 1.9);

        jacobian.setElementAt(12, 3, g11);
        jacobian.setElementAt(12, 6, g21);
        jacobian.setElementAt(12, 9, g31);
        jacobian.setElementAt(12, 12, m11);
        jacobian.setElementAt(12, 13, m12);
        jacobian.setElementAt(12, 14, m13);

        jacobian.setElementAt(13, 4, g11);
        jacobian.setElementAt(13, 7, g21);
        jacobian.setElementAt(13, 10, g31);
        jacobian.setElementAt(13, 12, m21);
        jacobian.setElementAt(13, 13, m22);
        jacobian.setElementAt(13, 14, m23);

        jacobian.setElementAt(14, 5, g11);
        jacobian.setElementAt(14, 8, g21);
        jacobian.setElementAt(14, 11, g31);
        jacobian.setElementAt(14, 12, m31);
        jacobian.setElementAt(14, 13, m32);
        jacobian.setElementAt(14, 14, m33);

        jacobian.setElementAt(15, 3, g12);
        jacobian.setElementAt(15, 6, g22);
        jacobian.setElementAt(15, 9, g32);
        jacobian.setElementAt(15, 15, m11);
        jacobian.setElementAt(15, 16, m12);
        jacobian.setElementAt(15, 17, m13);

        jacobian.setElementAt(16, 4, g12);
        jacobian.setElementAt(16, 7, g22);
        jacobian.setElementAt(16, 10, g32);
        jacobian.setElementAt(16, 15, m21);
        jacobian.setElementAt(16, 16, m22);
        jacobian.setElementAt(16, 17, m23);

        jacobian.setElementAt(17, 5, g12);
        jacobian.setElementAt(17, 8, g22);
        jacobian.setElementAt(17, 11, g32);
        jacobian.setElementAt(17, 15, m31);
        jacobian.setElementAt(17, 16, m32);
        jacobian.setElementAt(17, 17, m33);

        jacobian.setElementAt(18, 3, g13);
        jacobian.setElementAt(18, 6, g23);
        jacobian.setElementAt(18, 9, g33);
        jacobian.setElementAt(18, 18, m11);
        jacobian.setElementAt(18, 19, m12);
        jacobian.setElementAt(18, 20, m13);

        jacobian.setElementAt(19, 4, g13);
        jacobian.setElementAt(19, 7, g23);
        jacobian.setElementAt(19, 10, g33);
        jacobian.setElementAt(19, 18, m21);
        jacobian.setElementAt(19, 19, m22);
        jacobian.setElementAt(19, 20, m23);

        jacobian.setElementAt(20, 5, g13);
        jacobian.setElementAt(20, 8, g23);
        jacobian.setElementAt(20, 11, g33);
        jacobian.setElementAt(20, 18, m31);
        jacobian.setElementAt(20, 19, m32);
        jacobian.setElementAt(20, 20, m33);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and G-dependent cross biases
     * are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5354
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6092
jacobian.setElementAt(8, 5, 1.9);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter when G-dependent cross biases
     * are taken into account.
     *
     * @throws AlgebraException                              if provided accelerometer cross coupling
     *                                                       errors are not valid.
     * @throws InvalidSourceAndDestinationFrameTypeException never happens
     */
    private void setInputDataWithGDependentCrossBiases()
            throws AlgebraException, InvalidSourceAndDestinationFrameTypeException {
        // compute reference frame at current position
        final var nedPosition = getNedPosition();
        final var nedC = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var nedFrame = new NEDFrame(nedPosition, nedC);
        final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
        final var refKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
                ecefFrame);

        final var refAngularRateX = refKinematics.getAngularRateX();
        final var refAngularRateY = refKinematics.getAngularRateY();
        final var refAngularRateZ = refKinematics.getAngularRateZ();

        final var w2 = turntableRotationRate * turntableRotationRate;

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
        final var y = new double[numMeasurements];
        final var angularRateStandardDeviations = new double[numMeasurements];
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();

            final var angularRateX = measuredKinematics.getAngularRateX();
            final var angularRateY = measuredKinematics.getAngularRateY();
            final var angularRateZ = measuredKinematics.getAngularRateZ();

            final var fX = measuredKinematics.getFx();
            final var fY = measuredKinematics.getFy();
            final var fZ = measuredKinematics.getFz();

            x.setElementAt(i, 0, angularRateX - refAngularRateX);
            x.setElementAt(i, 1, angularRateY - refAngularRateY);
            x.setElementAt(i, 2, angularRateZ - refAngularRateZ);

            x.setElementAt(i, 3, fX);
            x.setElementAt(i, 4, fY);
            x.setElementAt(i, 5, fZ);

            y[i] = w2;

            angularRateStandardDeviations[i] = measurement.getAngularRateStandardDeviation();

            i++;
        }

        fitter.setInputData(x, y, angularRateStandardDeviations);

        ba = getAccelerometerBiasAsMatrix();
        ma = getAccelerometerMa();
        accelerationFixer.setBias(ba);
        accelerationFixer.setCrossCouplingErrors(ma);
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter when G-dependent cross biases
     * are ignored.
     *
     * @throws AlgebraException                              if provided accelerometer cross coupling
     *                                                       errors are not valid.
     * @throws InvalidSourceAndDestinationFrameTypeException never happens.
     */
    private void setInputData() throws AlgebraException, InvalidSourceAndDestinationFrameTypeException {

        // compute reference frame at current position
        final var nedPosition = getNedPosition();
        final var nedC = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var nedFrame = new NEDFrame(nedPosition, nedC);
        final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
        final var refKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
                ecefFrame);

        final var refAngularRateX = refKinematics.getAngularRateX();
        final var refAngularRateY = refKinematics.getAngularRateY();
        final var refAngularRateZ = refKinematics.getAngularRateZ();

        final var w2 = turntableRotationRate * turntableRotationRate;

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final var y = new double[numMeasurements];
        final var angularRateStandardDeviations = new double[numMeasurements];
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();

            final var angularRateX = measuredKinematics.getAngularRateX();
            final var angularRateY = measuredKinematics.getAngularRateY();
            final var angularRateZ = measuredKinematics.getAngularRateZ();

            x.setElementAt(i, 0, angularRateX - refAngularRateX);
            x.setElementAt(i, 1, angularRateY - refAngularRateY);
            x.setElementAt(i, 2, angularRateZ - refAngularRateZ);

            y[i] = w2;

            angularRateStandardDeviations[i] = measurement.getAngularRateStandardDeviation();

            i++;
        }

        fitter.setInputData(x, y, angularRateStandardDeviations);

        ba = getAccelerometerBiasAsMatrix();
        ma = getAccelerometerMa();
        accelerationFixer.setBias(ba);
        accelerationFixer.setCrossCouplingErrors(ma);
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final var velocity = new ECEFVelocity();
        final var result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(), 0.0, 0.0, 0.0,
                result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param value angular speed value.
     * @param unit  unit of angular speed value.
     * @return converted value.
     */
    private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
        return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }

    /**
     * Converts time instance to seconds.
     *
     * @param time time instance to be converted.
     * @return converted value.
     */
    private static double convertTime(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Makes proper conversion of internal cross-coupling and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix g) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4015
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1375
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1331
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2079
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1266
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1285
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        internalSetBias(bias);
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        internalSetBias(bias);
    }

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java 250
com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java 335
}
    }

    /**
     * Gets estimated average of x coordinate of measurement expressed in its default
     * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @return average of x coordinate of measurement in current window.
     */
    public double getAvgX() {
        return avgX;
    }

    /**
     * Gets estimated average of x coordinate of measurement within current window.
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @return average of x coordinate of measurement in current window.
     */
    public M getAvgXAsMeasurement() {
        return createMeasurement(avgX, getDefaultUnit());
    }

    /**
     * Gets estimated average of x coordinate of measurement within current window.
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @param result instance where average of x coordinate of measurement will be stored.
     */
    public void getAvgXAsMeasurement(final M result) {
        result.setValue(avgX);
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated average of y coordinate of measurement expressed in its default
     * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @return average of y coordinate of measurement in current window.
     */
    public double getAvgY() {
        return avgY;
    }

    /**
     * Gets estimated average of y coordinate of measurement within current window.
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @return average of y coordinate of measurement in current window.
     */
    public M getAvgYAsMeasurement() {
        return createMeasurement(avgY, getDefaultUnit());
    }

    /**
     * Gets estimated average of y coordinate of measurement within current window.
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @param result instance where average of y coordinate of measurement will be stored.
     */
    public void getAvgYAsMeasurement(final M result) {
        result.setValue(avgY);
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated average of z coordinate of measurement expressed in its default
     * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @return average of z coordinate of measurement in current window.
     */
    public double getAvgZ() {
        return avgZ;
    }

    /**
     * Gets estimated average of z coordinate of measurement within current window.
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @return average of z coordinate of measurement in current window.
     */
    public M getAvgZAsMeasurement() {
        return createMeasurement(avgZ, getDefaultUnit());
    }

    /**
     * Gets estimated average of z coordinate of measurement within current window.
     * This value will depend of body location and orientation, hence it should never
     * be used as a calibration bias.
     *
     * @param result instance where average of z coordinate of measurement will be stored.
     */
    public void getAvgZAsMeasurement(final M result) {
        result.setValue(avgZ);
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated average as a measurement triad.
     *
     * @return average measurement triad.
     */
    public T getAvgTriad() {
        return createTriad(avgX, avgY, avgZ, getDefaultUnit());
    }

    /**
     * Gets estimated average as a measurement triad.
     *
     * @param result instance where average values and unit will be stored.
     */
    public void getAvgTriad(final T result) {
        result.setValueCoordinatesAndUnit(avgX, avgY, avgZ, getDefaultUnit());
    }

    /**
     * Gets norm of estimated average measurement expressed in its default
     * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
     * This value is independent of body orientation.
     *
     * @return norm of estimated average specific force.
     */
    public double getAvgNorm() {
        return Math.sqrt(avgX * avgX + avgY * avgY + avgZ * avgZ);
    }

    /**
     * Gets norm of estimated average measurement within current window.
     *
     * @return norm of estimated average measurement.
     */
    public M getAvgNormAsMeasurement() {
        return createMeasurement(getAvgNorm(), getDefaultUnit());
    }

    /**
     * Gets norm of estimated average measurement within current window.
     *
     * @param result instance where norm of estimated average measurement will be stored.
     */
    public void getAvgNormAsMeasurement(final M result) {
        result.setValue(getAvgNorm());
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated variance of x coordinate of measurement within current window
     * expressed in its default squared unit (m^2/s^4 for acceleration,
     * rad^2/s^2 for angular speed or T^2 for magnetic flux density).
     *
     * @return estimated variance of x coordinate of measurement within current
     * window.
     */
    public double getVarianceX() {
        return varianceX;
    }

    /**
     * Gets estimated variance of y coordinate of measurement within current window
     * expressed in its default squared unit (m^2/s^4 for acceleration,
     * rad^2/s^2 for angular speed or T^2 for magnetic flux density).
     *
     * @return estimated variance of y coordinate of measurement within current
     * window.
     */
    public double getVarianceY() {
        return varianceY;
    }

    /**
     * Gets estimated variance of z coordinate of measurement within current window
     * expressed in its default squared unit (m^2/s^4 for acceleration,
     * rad^2/s^2 for angular speed or T^2 for magnetic flux density).
     *
     * @return estimated variance of z coordinate of measurement within current
     * window.
     */
    public double getVarianceZ() {
        return varianceZ;
    }

    /**
     * Gets estimated standard deviation of x coordinate of measurement within current
     * window and expressed in its default unit (m/s^2 for acceleration, rad/s for
     * angular speed or T for magnetic flux density).
     *
     * @return estimated standard deviation of x coordinate of measurement within
     * current window.
     */
    public double getStandardDeviationX() {
        return Math.sqrt(varianceX);
    }

    /**
     * Gets estimated standard deviation of x coordinate of measurement within current
     * window.
     *
     * @return estimated standard deviation of x coordinate of measurement.
     */
    public M getStandardDeviationXAsMeasurement() {
        return createMeasurement(getStandardDeviationX(), getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of x coordinate of measurement within current
     * window.
     *
     * @param result instance where estimated standard deviation of x coordinate of
     *               measurement will be stored.
     */
    public void getStandardDeviationXAsMeasurement(final M result) {
        result.setValue(getStandardDeviationX());
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of y coordinate of measurement within current
     * window and expressed in its default unit (m/s^2 for acceleration, rad/s for
     * angular speed or T for magnetic flux density).
     *
     * @return estimated standard deviation of y coordinate of measurement within
     * current window.
     */
    public double getStandardDeviationY() {
        return Math.sqrt(varianceY);
    }

    /**
     * Gets estimated standard deviation of y coordinate of measurement within current
     * window.
     *
     * @return estimated standard deviation of y coordinate of measurement.
     */
    public M getStandardDeviationYAsMeasurement() {
        return createMeasurement(getStandardDeviationY(), getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of y coordinate of measurement within current
     * window.
     *
     * @param result instance where estimated standard deviation of y coordinate of
     *               measurement will be stored.
     */
    public void getStandardDeviationYAsMeasurement(final M result) {
        result.setValue(getStandardDeviationY());
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of z coordinate of measurement within current
     * window and expressed in its default unit (m/s^2 for acceleration, rad/s for
     * angular speed or T for magnetic flux density).
     *
     * @return estimated standard deviation of z coordinate of measurement within
     * current window.
     */
    public double getStandardDeviationZ() {
        return Math.sqrt(varianceZ);
    }

    /**
     * Gets estimated standard deviation of z coordinate of measurement within current
     * window.
     *
     * @return estimated standard deviation of z coordinate of measurement.
     */
    public M getStandardDeviationZAsMeasurement() {
        return createMeasurement(getStandardDeviationZ(), getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of z coordinate of measurement within current
     * window.
     *
     * @param result instance where estimated standard deviation of z coordinate of
     *               measurement will be stored.
     */
    public void getStandardDeviationZAsMeasurement(final M result) {
        result.setValue(getStandardDeviationZ());
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of measurements.
     *
     * @return estimated standard deviation triad of measurements.
     */
    public T getStandardDeviationTriad() {
        return createTriad(getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ(), getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of measurement within current window.
     *
     * @param result instance where estimated standard deviation triad of
     *               measurement will be stored.
     */
    public void getStandardDeviationTriad(final T result) {
        result.setValueCoordinatesAndUnit(getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ(),
                getDefaultUnit());
    }

    /**
     * Gets norm of estimated standard deviation of measurement
     * expressed in its default unit (m/s^2 for acceleration, rad/s for
     * angular speed or T for magnetic flux density).
     *
     * @return norm of estimated standard deviation of measurement.
     */
    public double getStandardDeviationNorm() {
        final var fx = getStandardDeviationX();
        final var fy = getStandardDeviationY();
        final var fz = getStandardDeviationZ();
        return Math.sqrt(fx * fx + fy * fy + fz * fz);
    }

    /**
     * Gets norm of estimated standard deviation of measurements.
     *
     * @return norm of estimated standard deviation of measurement.
     */
    public M getStandardDeviationNormAsMeasurement() {
        return createMeasurement(getStandardDeviationNorm(), getDefaultUnit());
    }

    /**
     * Gets norm of estimated standard deviation of measurement within current window.
     *
     * @param result instance where norm of estimated standard deviation will be stored.
     */
    public void getStandardDeviationNormAsMeasurement(final M result) {
        result.setValue(getStandardDeviationNorm());
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets average of estimated standard deviation coordinates of measurement
     * expressed in its default unit (m/s^2 for acceleration, rad/s for
     * angular speed or T for magnetic flux density).
     *
     * @return average of estimated standard deviation coordinates.
     */
    public double getAverageStandardDeviation() {
        final var fx = getStandardDeviationX();
        final var fy = getStandardDeviationY();
        final var fz = getStandardDeviationZ();
        return (fx + fy + fz) / 3.0;
    }

    /**
     * Gets average of estimated standard deviation coordinates of measurement within
     * current window.
     *
     * @return average of estimated standard deviation coordinates.
     */
    public M getAverageStandardDeviationAsMeasurement() {
        return createMeasurement(getAverageStandardDeviation(), getDefaultUnit());
    }

    /**
     * Gets average of estimated standard deviation coordinates of measurement.
     *
     * @param result instance where average of estimated standard deviation coordinates
     *               will be stored.
     */
    public void getAverageStandardDeviationAsMeasurement(final M result) {
        result.setValue(getAverageStandardDeviation());
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets measurement noise PSD (Power Spectral Density) on x axis expressed
     * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
     * magnetometer.
     *
     * @return measurement noise PSD on x axis.
     */
    public double getPsdX() {
        return varianceX * timeInterval;
    }

    /**
     * Gets measurement noise PSD (Power Spectral Density) on y axis expressed
     * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
     * magnetometer.
     *
     * @return measurement noise PSD on y axis.
     */
    public double getPsdY() {
        return varianceY * timeInterval;
    }

    /**
     * Gets measurement noise PSD (Power Spectral Density) on z axis expressed
     * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
     * magnetometer.
     *
     * @return measurement noise PSD on z axis.
     */
    public double getPsdZ() {
        return varianceZ * timeInterval;
    }

    /**
     * Gets measurement noise root PSD (Power Spectral Density) on x axis expressed in
     * (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
     * magnetometer.
     *
     * @return measurement noise root PSD on x axis.
     */
    public double getRootPsdX() {
        return Math.sqrt(getPsdX());
    }

    /**
     * Gets measurement noise root PSD (Power Spectral Density) on y axis expressed in
     * (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
     * magnetometer.
     *
     * @return measurement noise root PSD on y axis.
     */
    public double getRootPsdY() {
        return Math.sqrt(getPsdY());
    }

    /**
     * Gets measurement noise root PSD (Power Spectral Density) on z axis expressed in
     * (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
     * magnetometer.
     *
     * @return measurement noise root PSD on z axis.
     */
    public double getRootPsdZ() {
        return Math.sqrt(getPsdZ());
    }

    /**
     * Gets average measurement noise PSD (Power Spectral Density) among
     * x,y,z components expressed as (m^2 * s^-3) for accelerometer,
     * (rad^2/s) for gyroscope or (T^2 * s) for magnetometer.
     *
     * @return average measurement noise PSD.
     */
    public double getAvgNoisePsd() {
        return (getPsdX() + getPsdY() + getPsdZ()) / 3.0;
    }

    /**
     * Gets norm of noise root PSD (Power Spectral Density) among x,y,z
     * components expressed as (m * s^-1.5) for accelerometer,
     * (rad * s^-0.5) for gyroscope or (T * s^0.5) for magnetometer.
     *
     * @return norm of measurement noise root PSD.
     */
    public double getNoiseRootPsdNorm() {
        return Math.sqrt(getPsdX() + getPsdY() + getPsdZ());
    }

    /**
     * Gets number of samples that have been processed so far.
     *
     * @return number of samples that have been processed so far.
     */
    public int getNumberOfProcessedSamples() {
        return numberOfProcessedSamples;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    public boolean isRunning() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3135
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4434
setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];

        final var m12 = result[1];
        final var m22 = result[2];

        final var m13 = result[3];
        final var m23 = result[4];
        final var m33 = result[5];

        final var g11 = result[6];
        final var g21 = result[7];
        final var g31 = result[8];

        final var g12 = result[9];
        final var g22 = result[10];
        final var g32 = result[11];

        final var g13 = result[12];
        final var g23 = result[13];
        final var g33 = result[14];

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, 0.0);
        mm.setElementAtIndex(2, 0.0);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, 0.0);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mg);

        // at this point covariance is expressed in terms of M and G, and must
        // be expressed in terms of Mg and Gg.
        // We know that:
        // Mg = M - I
        // Gg = M * G

        // M = [m11  m12  m13]
        //     [0    m22  m23]
        //     [0    0    m33]

        // G = [g11  g12  g13]
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [0          m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [0          0           m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [0    m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [0    0    m33][g31  g32  g33]

        // Defining the linear application:
        // F(M, G) = F(m11, m12, m22, m32=0, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
        // as:
        // [sx] =   [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]
        // [gg11]   [m11 * g11 + m12 * g21 + m13 * g31]
        // [gg21]   [            m22 * g21 + m23 * g31]
        // [gg31]   [                        m33 * g31]
        // [gg12]   [m11 * g12 + m12 * g22 + m13 * g32]
        // [gg22]   [            m22 * g22 + n23 * g32]
        // [gg32]   [                        m33 * g32]
        // [gg13]   [m11 * g13 + m12 * g23 + m13 * g33]
        // [gg23]   [            m22 * g23 + m23 * g33]
        // [gg33]   [                        m33 * g33]

        // Then the Jacobian of F(M, G) is:
        // J = [1    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    1    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0    0    0    0  ]
        //     [0    1    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    1    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [g11  g21  0    g31  0    0    m11  m12  m13  0    0    0    0    0    0  ]
        //     [0    0    g21  0    g31  0    0    m22  m23  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    g31  0    0    m33  0    0    0    0    0    0  ]
        //     [g12  g22  0    g32  0    0    0    0    0    m11  m12  m13  0    0    0  ]
        //     [0    0    g22  0    g32  0    0    0    0    0    m22  m23  0    0    0  ]
        //     [0    0    0    0    0    g32  0    0    0    0    0    m33  0    0    0  ]
        //     [g13  g23  0    g33  0    0    0    0    0    0    0    0    m11  m12  m13]
        //     [0    0    g23  0    g33  0    0    0    0    0    0    0    0    m22  m23]
        //     [0    0    0    0    0    g33  0    0    0    0    0    0    0    0    m33]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);

        jacobian.setElementAt(0, 0, 1.0);
        jacobian.setElementAt(1, 2, 1.0);
        jacobian.setElementAt(2, 5, 1.0);

        jacobian.setElementAt(3, 1, 1.0);
        jacobian.setElementAt(4, 3, 1.0);

        jacobian.setElementAt(6, 4, 1.0);

        jacobian.setElementAt(9, 0, g11);
        jacobian.setElementAt(9, 1, g21);
        jacobian.setElementAt(9, 3, g31);
        jacobian.setElementAt(9, 6, m11);
        jacobian.setElementAt(9, 7, m12);
        jacobian.setElementAt(9, 8, m13);

        jacobian.setElementAt(10, 2, g21);
        jacobian.setElementAt(10, 4, g31);
        jacobian.setElementAt(10, 7, m22);
        jacobian.setElementAt(10, 8, m23);

        jacobian.setElementAt(11, 5, g31);
        jacobian.setElementAt(11, 8, m33);

        jacobian.setElementAt(12, 0, g12);
        jacobian.setElementAt(12, 1, g22);
        jacobian.setElementAt(12, 3, g32);
        jacobian.setElementAt(12, 9, m11);
        jacobian.setElementAt(12, 10, m12);
        jacobian.setElementAt(12, 11, m13);

        jacobian.setElementAt(13, 2, g22);
        jacobian.setElementAt(13, 4, g32);
        jacobian.setElementAt(13, 10, m22);
        jacobian.setElementAt(13, 11, m23);

        jacobian.setElementAt(14, 5, g32);
        jacobian.setElementAt(14, 11, m33);

        jacobian.setElementAt(15, 0, g13);
        jacobian.setElementAt(15, 1, g23);
        jacobian.setElementAt(15, 3, g33);
        jacobian.setElementAt(15, 12, m11);
        jacobian.setElementAt(15, 13, m12);
        jacobian.setElementAt(15, 14, m13);

        jacobian.setElementAt(16, 2, g23);
        jacobian.setElementAt(16, 4, g33);
        jacobian.setElementAt(16, 13, m22);
        jacobian.setElementAt(16, 14, m23);

        jacobian.setElementAt(17, 5, g33);
        jacobian.setElementAt(17, 14, m33);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform general calibration when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneralAndGDependentCrossBiases() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2865
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 480
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3225
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4157
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2870
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3230
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2982
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4162
}

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 634
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3717
} catch (final AlgebraException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return estimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (estimatedHardIron != null) {
            System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedHardIron != null) {
            result.fromArray(estimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return estimatedHardIron != null ? estimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return estimatedHardIron != null ? estimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return estimatedHardIron != null ? estimatedHardIron[2] : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @return x coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[0]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @return y coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[1]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @return z coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[2]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @return estimated magnetometer bias or null if not available.
     */
    @Override
    public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
        return estimatedHardIron != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2]) : null;
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
        if (estimatedHardIron != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2], MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5002
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 468
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2977
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFy() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFz() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @return estimated accelerometer bias or null if not available.
     */
    @Override
    public AccelerationTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null ?
                new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                        estimatedBiases[0], estimatedBiases[1], estimatedBiases[2]) : null;
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated accelerometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }


    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2141
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 634
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return estimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (estimatedHardIron != null) {
            System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedHardIron != null) {
            result.fromArray(estimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return estimatedHardIron != null ? estimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return estimatedHardIron != null ? estimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return estimatedHardIron != null ? estimatedHardIron[2] : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @return x coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[0]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @return y coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[1]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @return z coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[2]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @return estimated magnetometer bias or null if not available.
     */
    @Override
    public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
        return estimatedHardIron != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
                : null;
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
        if (estimatedHardIron != null) {
            result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2870
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3230
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1693
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4243
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4162
}

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z components of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per
     * second (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 485
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2982
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1693
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4243
}

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where estimated gyroscope biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return column matrix containing x,y,z component of estimated gyroscope
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasX() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasY() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasZ() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @return x coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedX() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @return y coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedY() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @return z coordinate of estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeed getEstimatedBiasAngularSpeedZ() {
        return estimatedBiases != null
                ? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @return estimated gyroscope bias or null if not available.
     */
    @Override
    public AngularSpeedTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated gyroscope bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5007
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2982
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1632
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2290
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2249
}

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFy() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFz() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return estimatedBiases != null ?
                new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @return estimated accelerometer bias or null if not available.
     */
    @Override
    public AccelerationTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null ?
                new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                        estimatedBiases[0], estimatedBiases[1], estimatedBiases[2]) : null;
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated accelerometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }


    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2146
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3722
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1658
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2379
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2512
}

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return estimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (estimatedHardIron != null) {
            System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedHardIron != null) {
            result.fromArray(estimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return estimatedHardIron != null ? estimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return estimatedHardIron != null ? estimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return estimatedHardIron != null ? estimatedHardIron[2] : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @return x coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[0]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @return y coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[1]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @return z coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[2]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @return estimated magnetometer bias or null if not available.
     */
    @Override
    public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
        return estimatedHardIron != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
                : null;
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
        if (estimatedHardIron != null) {
            result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 473
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1632
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2290
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2249
}

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
        return estimatedBiases != null ? estimatedBiases[0] : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFy() {
        return estimatedBiases != null ? estimatedBiases[1] : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFz() {
        return estimatedBiases != null ? estimatedBiases[2] : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFxAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets x coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[0]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @return y coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFyAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets y coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[1]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @return z coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Acceleration getEstimatedBiasFzAsAcceleration() {
        return estimatedBiases != null
                ? new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets z coordinate of estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if result was updated, false if estimation is not available.
     */
    @Override
    public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
        if (estimatedBiases != null) {
            result.setValue(estimatedBiases[2]);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @return estimated accelerometer bias or null if not available.
     */
    @Override
    public AccelerationTriad getEstimatedBiasAsTriad() {
        return estimatedBiases != null
                ? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
                : null;
    }

    /**
     * Gets estimated accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated accelerometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
        if (estimatedBiases != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and cross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 639
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1658
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2379
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2512
}

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
        return estimatedHardIron;
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where estimated magnetometer biases will be
     *               stored.
     * @return true if result instance was updated, false otherwise (when
     * estimation is not yet available).
     */
    @Override
    public boolean getEstimatedHardIron(final double[] result) {
        if (estimatedHardIron != null) {
            System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @return column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases.
     */
    @Override
    public Matrix getEstimatedHardIronAsMatrix() {
        return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated
     * magnetometer hard-iron biases expressed in Teslas (T).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedHardIron != null) {
            result.fromArray(estimatedHardIron);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return x coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronX() {
        return estimatedHardIron != null ? estimatedHardIron[0] : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return y coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronY() {
        return estimatedHardIron != null ? estimatedHardIron[1] : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias expressed in
     * Teslas (T).
     *
     * @return z coordinate of estimated magnetometer bias or null if not
     * available.
     */
    @Override
    public Double getEstimatedHardIronZ() {
        return estimatedHardIron != null ? estimatedHardIron[2] : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @return x coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets x coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[0]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @return y coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets y coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[1]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @return z coordinate of estimated magnetometer bias.
     */
    @Override
    public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
        return estimatedHardIron != null
                ? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets z coordinate of estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedHardIron != null) {
            result.setValue(estimatedHardIron[2]);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @return estimated magnetometer bias or null if not available.
     */
    @Override
    public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
        return estimatedHardIron != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2]) : null;
    }

    /**
     * Gets estimated magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if estimated magnetometer bias is available and result was
     * modified, false otherwise.
     */
    @Override
    public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
        if (estimatedHardIron != null) {
            result.setValueCoordinatesAndUnit(
                    estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2], MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1924
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1952
hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return position;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (position != null) {
            final var result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {

        if (position != null) {
            final var velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    position.getLatitude(), position.getLongitude(), position.getHeight(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return year;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.year = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseconds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        year = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        year = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        year = convertTime(calendar);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public List<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
        return measurements;
    }

    /**
     * Sets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @param measurements collection of body magnetic flux density
     *                     measurements at a known position and timestamp
     *                     with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public MagnetometerCalibratorMeasurementType getMeasurementType() {
        return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3380
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4642
}

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredMeasurementsOrSequences}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurementsOrSequences()) {
            throw new IllegalArgumentException();
        }

        this.preliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustEasyGyroscopeCalibrator create(final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3556
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3567
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return turntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        this.turntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
        result.setValue(turntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3515
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3603
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @return constant rotation rate of turntable.
     */
    public double getTurntableRotationRate() {
        return turntableRotationRate;
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     * This is expressed in radians per second (rad/s).
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(
            final double turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (turntableRotationRate <= 0.0) {
            throw new IllegalArgumentException();
        }

        this.turntableRotationRate = turntableRotationRate;
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @return constant rotation rate of turntable.
     */
    public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
        return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets constant rotation rate at which the turntable is spinning.
     *
     * @param result instance where result will be stored.
     */
    public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
        result.setValue(turntableRotationRate);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets constant rotation rate at which the turntable is spinning.
     *
     * @param turntableRotationRate constant rotation rate of turntable.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
    }

    /**
     * Gets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @return time interval between measurements.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between measurements being captured expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is zero or
     *                                  negative.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval <= 0.0) {
            throw new IllegalArgumentException();
        }
        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @return time interval between measurements.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between measurements being captured.
     *
     * @param result instance where result will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between measurements being captured.
     *
     * @param timeInterval time interval between measurements.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public List<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3380
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2086
}

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredMeasurementsOrSequences}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurementsOrSequences()) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3257
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3610
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3368
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4549
public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 901
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 903
this(convertPosition(position), measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken.
     *
     * @return position where body magnetic flux density measurements have
     * been taken.
     */
    public NEDPosition getNedPosition() {
        return position;
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (position != null) {
            final var result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {
        if (position != null) {
            final var velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    position.getLatitude(), position.getLongitude(), position.getHeight(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }

        this.position = convertPosition(position);
    }

    /**
     * Gets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @return timestamp expressed as decimal year or null if not defined.
     */
    public Double getYear() {
        return year;
    }

    /**
     * Sets timestamp expressed as decimal year where magnetic flux density
     * measurements have been measured.
     *
     * @param year timestamp expressed as decimal year.
     * @throws LockedException if calibrator is currently running.
     */
    public void setYear(final Double year) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        this.year = year;
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param timestampMillis a timestamp expressed in milliseconds since
     *                        epoch time (January 1st, 1970 at midnight).
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Long timestampMillis) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        year = convertTime(timestampMillis);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param date a date instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final Date date) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        year = convertTime(date);
    }

    /**
     * Sets timestamp when magnetic flux density measurements have been
     * measured.
     *
     * @param calendar a calendar instance containing a timestamp.
     * @throws LockedException if calibrator is currently running.
     */
    public void setTime(final GregorianCalendar calendar) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        year = convertTime(calendar);
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && position != null && year != null;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return magneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        this.magneticModel = magneticModel;
    }

    /**
     * Called before calibration occurs.
     * This can be overridden by subclasses.
     *
     * @throws CalibrationException if anything fails.
     */
    @Override
    protected void onBeforeCalibrate() throws CalibrationException {
        computeGroundTruthMagneticFluxDensityNorm();
    }

    /**
     * Computes expected ground truth magnetic flux density norm based on World Magnetic Model.
     * Notice that actual magnetic field measured by devices might differ from this value, since it might be attenuated
     * or other devices might interfere.
     *
     * @throws CalibrationException if world magnetic model cannot be loaded.
     */
    private void computeGroundTruthMagneticFluxDensityNorm() throws CalibrationException {
        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (magneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
        } else {
            try {
                wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
            } catch (final IOException e) {
                throw new CalibrationException(e);
            }
        }

        final var pos = getNedPosition();
        final var earthB = wmmEstimator.estimate(pos, year);
        groundTruthMagneticFluxDensityNorm = earthB.getNorm();
    }

    /**
     * Converts a time instance expressed in milliseconds since epoch time
     * (January 1st, 1970 at midnight) to a decimal year.
     *
     * @param timestampMillis milliseconds value to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Long timestampMillis) {
        if (timestampMillis == null) {
            return null;
        }

        final var calendar = new GregorianCalendar();
        calendar.setTimeInMillis(timestampMillis);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained ina date object to a
     * decimal year.
     *
     * @param date a time instance to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Date date) {
        if (date == null) {
            return null;
        }

        final var calendar = new GregorianCalendar();
        calendar.setTime(date);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained in a gregorian calendar to a
     * decimal year.
     *
     * @param calendar calendar containing a specific instant to be
     *                 converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final GregorianCalendar calendar) {
        if (calendar == null) {
            return null;
        }

        return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
    }

    /**
     * Converts provided ECEF position to position expressed in NED
     * coordinates.
     *
     * @param position ECEF position to be converted.
     * @return converted position expressed in NED coordinates.
     */
    private static NEDPosition convertPosition(final ECEFPosition position) {
        final var velocity = new NEDVelocity();
        final var result = new NEDPosition();
        ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                position.getX(), position.getY(), position.getZ(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4409
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4743
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements collection of body kinematics measurements at a
     *                     known position with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<StandardDeviationBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public AccelerometerCalibratorMeasurementType getMeasurementType() {
        return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public L getListener() {
        return listener;
    }

    /**
     * Sets listener to handle events raised by this estimator.
     *
     * @param listener listener to handle events raised by this estimator.
     * @throws LockedException if calibrator is currently running.
     */
    public void setListener(final L listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
                && groundTruthGravityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                //noinspection unchecked
                listener.onCalibrateStart((C) this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                //noinspection unchecked
                listener.onCalibrateEnd((C) this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1797
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1879
hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
        return measurements;
    }

    /**
     * Sets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @param measurements collection of body magnetic flux density
     *                     measurements at a known position and timestamp
     *                     with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<StandardDeviationBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public MagnetometerCalibratorMeasurementType getMeasurementType() {
        return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public L getListener() {
        return listener;
    }

    /**
     * Sets listener to handle events raised by this calibrator.
     *
     * @param listener listener to handle events raised by this calibrator.
     * @throws LockedException if calibrator is currently running.
     */
    public void setListener(final L listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurements();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates magnetometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            onBeforeCalibrate();

            if (listener != null) {
                //noinspection unchecked
                listener.onCalibrateStart((C) this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                //noinspection unchecked
                listener.onCalibrateEnd((C) this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2064
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2455
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2061
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3125
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3164
public void setInitialBias(
            final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAngularSpeed(initialBiasX);
        this.initialBiasY = convertAngularSpeed(initialBiasY);
        this.initialBiasZ = convertAngularSpeed(initialBiasZ);
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3902
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4606
if (estimatedMg == null) {
            estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedMg.initialize(0.0);
        }

        estimatedMg.setElementAt(0, 0, sx);
        estimatedMg.setElementAt(1, 0, myx);
        estimatedMg.setElementAt(2, 0, mzx);

        estimatedMg.setElementAt(0, 1, mxy);
        estimatedMg.setElementAt(1, 1, sy);
        estimatedMg.setElementAt(2, 1, mzy);

        estimatedMg.setElementAt(0, 2, mxz);
        estimatedMg.setElementAt(1, 2, myz);
        estimatedMg.setElementAt(2, 2, sz);

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedGg.setElementAtIndex(0, g11);
        estimatedGg.setElementAtIndex(1, g21);
        estimatedGg.setElementAtIndex(2, g31);
        estimatedGg.setElementAtIndex(3, g12);
        estimatedGg.setElementAtIndex(4, g22);
        estimatedGg.setElementAtIndex(5, g32);
        estimatedGg.setElementAtIndex(6, g13);
        estimatedGg.setElementAtIndex(7, g23);
        estimatedGg.setElementAtIndex(8, g33);

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     */
    private void setInputData() throws WrongSizeException {
        // set input data using:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        final var expectedKinematics = new BodyKinematics();

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
        final var y = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final var standardDeviations = new double[numMeasurements];
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            x.setElementAt(i, 0, omegaTrueX);
            x.setElementAt(i, 1, omegaTrueY);
            x.setElementAt(i, 2, omegaTrueZ);

            x.setElementAt(i, 3, fTrueX);
            x.setElementAt(i, 4, fTrueY);
            x.setElementAt(i, 5, fTrueZ);

            y.setElementAt(i, 0, omegaMeasX);
            y.setElementAt(i, 1, omegaMeasY);
            y.setElementAt(i, 2, omegaMeasZ);

            standardDeviations[i] = measurement.getAngularRateStandardDeviation();
            i++;
        }

        fitter.setInputData(x, y, standardDeviations);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param value angular speed value.
     * @param unit  unit of angular speed value.
     * @return converted value.
     */
    private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
        return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2766
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4002
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than
     *                                  {@link #getMinimumRequiredMeasurementsOrSequences}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurementsOrSequences()) {
            throw new IllegalArgumentException();
        }

        this.preliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5384
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3360
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 2011
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2669
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2628
}

    /**
     * Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated x coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFxVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFxStandardDeviation() {
        final var variance = getEstimatedBiasFxVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated x coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFxStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated y coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFyVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFyStandardDeviation() {
        final var variance = getEstimatedBiasFyVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated y coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFyStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated z coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFzVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Double getEstimatedBiasFzStandardDeviation() {
        final var variance = getEstimatedBiasFzVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @return standard deviation of estimated z coordinate of accelerometer bias or null if not
     * available.
     */
    public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of accelerometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasFzStandardDeviation());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @return standard deviation of estimated accelerometer bias coordinates.
     */
    public AccelerationTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null ?
                new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                        getEstimatedBiasFxStandardDeviation(),
                        getEstimatedBiasFyStandardDeviation(),
                        getEstimatedBiasFzStandardDeviation()) : null;
    }

    /**
     * Gets standard deviation of estimated accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of accelerometer bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasFxStandardDeviation(),
                    getEstimatedBiasFyStandardDeviation(),
                    getEstimatedBiasFzStandardDeviation(),
                    AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates expressed
     * in meters per squared second (m/s^2).
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null ?
                (getEstimatedBiasFxStandardDeviation() + getEstimatedBiasFyStandardDeviation()
                        + getEstimatedBiasFzStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @return average of estimated standard deviation of accelerometer bias coordinates or null.
     */
    public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
        return estimatedCovariance != null ?
                new Acceleration(getEstimatedBiasStandardDeviationAverage(),
                        AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets average of estimated standard deviation of accelerometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of accelerometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias expressed in
     * meters per squared second (m/s^2).
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of accelerometer bias or null
     * if not available.
     */
    public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
        return estimatedCovariance != null
                ? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of accelerometer bias coordinates.
     * This can be used as the initial accelerometer bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of accelerometer bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3269
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3622
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2086
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4642
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4561
}

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasXStandardDeviation() {
        final var variance = getEstimatedBiasXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated x coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasXStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated y coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasYStandardDeviation() {
        final var variance = getEstimatedBiasYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated y coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasYStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated z coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
     * radians per second (rad/s).
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public Double getEstimatedBiasZStandardDeviation() {
        final var variance = getEstimatedBiasZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @return standard deviation of estimated z coordinate of gyroscope bias or null if not
     * available.
     */
    public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of gyroscope bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasZStandardDeviation());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @return standard deviation of estimated gyroscope bias coordinates.
     */
    public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
        return estimatedCovariance != null
                ? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
                getEstimatedBiasXStandardDeviation(),
                getEstimatedBiasYStandardDeviation(),
                getEstimatedBiasZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of gyroscope bias was available, false
     * otherwise.
     */
    public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedBiasXStandardDeviation(),
                    getEstimatedBiasYStandardDeviation(),
                    getEstimatedBiasZStandardDeviation(),
                    AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates expressed
     * in radians per second (rad/s).
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null
     * if not available.
     */
    public Double getEstimatedBiasStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
                + getEstimatedBiasZStandardDeviation()) / 3.0 : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @return average of estimated standard deviation of gyroscope bias coordinates or null.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of gyroscope bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of gyroscope bias is available,
     * false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationAverage());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias expressed in
     * radians per second (rad/s).
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    @Override
    public Double getEstimatedBiasStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @return norm of estimated standard deviation of gyroscope bias or null
     * if not available.
     */
    public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
        return estimatedCovariance != null
                ? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of gyroscope bias coordinates.
     * This can be used as the initial gyroscope bias uncertainty for
     * {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of gyroscope bias is
     * available, false otherwise.
     */
    public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedBiasStandardDeviationNorm());
            result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
            return true;
        } else {
            return false;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2529
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4105
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 2040
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2762
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2895
}

    /**
     * Gets variance of estimated x coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated x coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronXVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronXStandardDeviation() {
        final var variance = getEstimatedHardIronXVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated x coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated x coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated x coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(
            final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronXStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated y coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated y coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronYVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronYStandardDeviation() {
        final var variance = getEstimatedHardIronYVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated y coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated y coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated y coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(
            final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronYStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets variance of estimated z coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated z coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronZVariance() {
        return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias
     * expressed in Teslas (T).
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public Double getEstimatedHardIronZStandardDeviation() {
        final var variance = getEstimatedHardIronZVariance();
        return variance != null ? Math.sqrt(variance) : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @return standard deviation of estimated z coordinate of magnetometer bias
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets standard deviation of estimated z coordinate of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of estimated z coordinate of
     * magnetometer bias is available, false otherwise.
     */
    public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronZStandardDeviation());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @return standard deviation of estimated magnetometer bias coordinates.
     */
    public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
        return estimatedCovariance != null
                ? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
                getEstimatedHardIronXStandardDeviation(),
                getEstimatedHardIronYStandardDeviation(),
                getEstimatedHardIronZStandardDeviation())
                : null;
    }

    /**
     * Gets standard deviation of estimated magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if standard deviation of magnetometer bias was available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
        if (estimatedCovariance != null) {
            result.setValueCoordinatesAndUnit(
                    getEstimatedHardIronXStandardDeviation(),
                    getEstimatedHardIronYStandardDeviation(),
                    getEstimatedHardIronZStandardDeviation(),
                    MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates
     * expressed in Teslas (T).
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public Double getEstimatedHardIronStandardDeviationAverage() {
        return estimatedCovariance != null
                ? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
                + getEstimatedHardIronZStandardDeviation()) / 3.0
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @return average of estimated standard deviation of magnetometer bias coordinates,
     * or null if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets average of estimated standard deviation of magnetometer bias coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if average of estimated standard deviation of magnetometer bias is available,
     * false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationAverage());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias expressed in
     * Teslas (T).
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public Double getEstimatedHardIronStandardDeviationNorm() {
        return estimatedCovariance != null
                ? Math.sqrt(getEstimatedHardIronXVariance() + getEstimatedHardIronYVariance()
                + getEstimatedHardIronZVariance()) : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @return norm of estimated standard deviation of magnetometer bias or null
     * if not available.
     */
    public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
        return estimatedCovariance != null
                ? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
                : null;
    }

    /**
     * Gets norm of estimated standard deviation of magnetometer bias.
     *
     * @param result instance where result will be stored.
     * @return true if norm of estimated standard deviation of magnetometer bias
     * is available, false otherwise.
     */
    public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(
            final MagneticFluxDensity result) {
        if (estimatedCovariance != null) {
            result.setValue(getEstimatedHardIronStandardDeviationNorm());
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2055
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2013
&& groundTruthGravityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #getMinimumRequiredMeasurements()}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #getMinimumRequiredMeasurements()}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        this.preliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 708
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1823
this.listener = listener;
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(biasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x coordinate of accelerometer bias.
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAcceleration(biasX);
    }

    /**
     * Gets known y coordinate of accelerometer bias.
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(biasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y coordinate of accelerometer bias.
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = convertAcceleration(biasY);
    }

    /**
     * Gets known z coordinate of accelerometer bias.
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(biasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z coordinate of accelerometer bias.
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known accelerometer bias coordinates expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @param biasY y coordinate of accelerometer bias.
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known accelerometer bias coordinates.
     *
     * @param biasX z coordinate of accelerometer bias.
     * @param biasY y coordinate of accelerometer bias.
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasCoordinates(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAcceleration(biasX);
        this.biasY = convertAcceleration(biasY);
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @return known accelerometer bias.
     */
    @Override
    public AccelerationTriad getBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known accelerometer bias.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBias(final AccelerationTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
        biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
        biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3714
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5033
setResult(m, b, g);

        // at this point covariance is expressed in terms of b, M and G, and must
        // be expressed in terms of bg, Mg and Gg.
        // We know that:
        // bg = M * b
        // Mg = M - I
        // Gg = M * G

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11  m12  m13]
        //     [0    m22  m23]
        //     [0    0    m33]

        // G = [g11  g12  g13]
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // bg = [m11  m12  m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
        //      [0    m22  m23][by]   [           m22 * by + m23 * bz]   [bgy]
        //      [0    0    m33][bz]   [                      m33 * bz]   [bgz]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [0          m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [0          0           m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [0    m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [0    0    m33][g31  g32  g33]

        // Defining the linear application:
        // F(b, M, G) = F(bx, by, bz, m11, m12, m22, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
        // as:
        // [bgx] =  [m11 * bx + m12 * by + m13 * bz]
        // [bgy]    [           m22 * by + m23 * bz]
        // [bgz]    [                      m33 * bz]
        // [sx]     [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]
        // [gg11]   [m11 * g11 + m12 * g21 + m13 * g31]
        // [gg21]   [            m22 * g21 + m23 * g31]
        // [gg31]   [                        m33 * g31]
        // [gg12]   [m11 * g12 + m12 * g22 + m13 * g32]
        // [gg22]   [            m22 * g22 + m23 * g32]
        // [gg32]   [                        m33 * g32]
        // [gg13]   [m11 * g13 + m12 * g23 + m13 * g33]
        // [gg23]   [            m22 * g23 + m23 * g33]
        // [gg33]   [                        m33 * g33]

        // Then the Jacobian of F(b, M, G) is:
        // J = [m11  m12  m13  bx   by   0    bz   0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    m22  m23  0    0    by   0    bz   0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    m33  0    0    0    0    0    bz   0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    g11  g21  0    g31  0    0    m11  m12  m13  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    g21  0    g31  0    0    m22  m23  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    g31  0    0    m33  0    0    0    0    0    0  ]
        //     [0    0    0    g12  g22  0    g32  0    0    0    0    0    m11  m12  m13  0    0    0  ]
        //     [0    0    0    0    0    g22  0    g32  0    0    0    0    0    m22  m23  0    0    0  ]
        //     [0    0    0    0    0    0    0    0    g32  0    0    0    0    0    m33  0    0    0  ]
        //     [0    0    0    g13  g23  0    g33  0    0    0    0    0    0    0    0    m11  m12  m13]
        //     [0    0    0    0    0    g23  0    g33  0    0    0    0    0    0    0    0    m22  m23]
        //     [0    0    0    0    0    0    0    0    g33  0    0    0    0    0    0    0    0    m33]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);

        jacobian.setElementAt(0, 0, m11);
        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(0, 4, by);
        jacobian.setElementAt(0, 6, bz);

        jacobian.setElementAt(1, 1, m22);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(1, 5, by);
        jacobian.setElementAt(1, 7, bz);

        jacobian.setElementAt(2, 2, m33);
        jacobian.setElementAt(2, 8, bz);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 5, 1.0);
        jacobian.setElementAt(5, 8, 1.0);

        jacobian.setElementAt(6, 4, 1.0);
        jacobian.setElementAt(7, 6, 1.0);

        jacobian.setElementAt(9, 7, 1.0);

        jacobian.setElementAt(12, 3, g11);
        jacobian.setElementAt(12, 4, g21);
        jacobian.setElementAt(12, 6, g31);
        jacobian.setElementAt(12, 9, m11);
        jacobian.setElementAt(12, 10, m12);
        jacobian.setElementAt(12, 11, m13);

        jacobian.setElementAt(13, 5, g21);
        jacobian.setElementAt(13, 7, g31);
        jacobian.setElementAt(13, 10, m22);
        jacobian.setElementAt(13, 11, m23);

        jacobian.setElementAt(14, 8, g31);
        jacobian.setElementAt(14, 11, m33);

        jacobian.setElementAt(15, 3, g12);
        jacobian.setElementAt(15, 4, g22);
        jacobian.setElementAt(15, 6, g32);
        jacobian.setElementAt(15, 12, m11);
        jacobian.setElementAt(15, 13, m12);
        jacobian.setElementAt(15, 14, m13);

        jacobian.setElementAt(16, 5, g22);
        jacobian.setElementAt(16, 7, g32);
        jacobian.setElementAt(16, 13, m22);
        jacobian.setElementAt(16, 14, m23);

        jacobian.setElementAt(17, 8, g32);
        jacobian.setElementAt(17, 14, m33);

        jacobian.setElementAt(18, 3, g13);
        jacobian.setElementAt(18, 4, g23);
        jacobian.setElementAt(18, 6, g33);
        jacobian.setElementAt(18, 15, m11);
        jacobian.setElementAt(18, 16, m12);
        jacobian.setElementAt(18, 17, m13);

        jacobian.setElementAt(19, 5, g23);
        jacobian.setElementAt(19, 7, g33);
        jacobian.setElementAt(19, 16, m22);
        jacobian.setElementAt(19, 17, m23);

        jacobian.setElementAt(20, 8, g33);
        jacobian.setElementAt(20, 17, m33);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform general calibration when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneralAndGDependentCrossBiases() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1328
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2722
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 480
this(measurements, commonAxisUsed, magneticModel, hardIron);
        this.listener = listener;
    }

    /**
     * Gets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return x-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets x-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX x coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return y-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets y-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY y coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return z-coordinate of known magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets z-coordinate of known magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronZ z coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed
     * in Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<FrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 3758
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 709
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1118
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1074
}

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(biasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(biasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAcceleration(biasY);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(biasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets z-coordinate of known accelerometer bias to be used to find a solution.
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known bias coordinates of accelerometer expressed in meters
     * per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known bias coordinates of accelerometer.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
        this.biasY = convertAcceleration(biasY);
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @return known accelerometer bias.
     */
    @Override
    public AccelerationTriad getBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known accelerometer bias.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBias(final AccelerationTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
        biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
        biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2171
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2305
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.preliminarySubsetSize = preliminarySubsetSize;
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    public abstract RobustEstimatorMethod getMethod();

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1330
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1299
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1264
}

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
        return hardIronX;
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronX known x-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final double hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
    }

    /**
     * Gets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known y-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronY() {
        return hardIronY;
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @param hardIronY known y-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final double hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = hardIronY;
    }

    /**
     * Gets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known z-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronZ() {
        return hardIronZ;
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron bias.
     * This is expressed in meters Teslas (T).
     *
     * @param hardIronZ known z-coordinate of magnetometer
     *                  hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = hardIronZ;
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @return x coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known x coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronX);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known x-coordinate of magnetometer hard-iron.
     *
     * @param hardIronX known x-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @return y coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known y coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronY);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known y-coordinate of magnetometer hard-iron.
     *
     * @param hardIronY known y-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @return z coordinate of magnetometer hard-iron.
     */
    @Override
    public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
        return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Gets known z coordinate of magnetometer hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
        result.setValue(hardIronZ);
        result.setUnit(MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known z-coordinate of magnetometer hard-iron.
     *
     * @param hardIronZ known z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Sets known hard-iron bias coordinates of magnetometer expressed in
     * Teslas (T).
     *
     * @param hardIronX x-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronY y-coordinate of magnetometer
     *                  known hard-iron bias.
     * @param hardIronZ z-coordinate of magnetometer
     *                  known hard-iron bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = hardIronX;
        this.hardIronY = hardIronY;
        this.hardIronZ = hardIronZ;
    }

    /**
     * Sets known hard-iron coordinates.
     *
     * @param hardIronX x-coordinate of magnetometer hard-iron.
     * @param hardIronY y-coordinate of magnetometer hard-iron.
     * @param hardIronZ z-coordinate of magnetometer hard-iron.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIronCoordinates(
            final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
            final MagneticFluxDensity hardIronZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.hardIronX = convertMagneticFluxDensity(hardIronX);
        this.hardIronY = convertMagneticFluxDensity(hardIronY);
        this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @return known hard-iron.
     */
    @Override
    public MagneticFluxDensityTriad getHardIronAsTriad() {
        return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
    }

    /**
     * Gets known hard-iron.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
        result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Sets known hard-iron.
     *
     * @param hardIron hard-iron to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
        hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
        hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4873
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4265
if (estimatedMm == null) {
            estimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
        } else {
            estimatedMm.initialize(0.0);
        }

        estimatedMm.setElementAt(0, 0, sx);
        estimatedMm.setElementAt(1, 0, myx);
        estimatedMm.setElementAt(2, 0, mzx);

        estimatedMm.setElementAt(0, 1, mxy);
        estimatedMm.setElementAt(1, 1, sy);
        estimatedMm.setElementAt(2, 1, mzy);

        estimatedMm.setElementAt(0, 2, mxz);
        estimatedMm.setElementAt(1, 2, myz);
        estimatedMm.setElementAt(2, 2, sz);

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     * @throws IOException        if world magnetic model cannot be loaded.
     */
    private void setInputData() throws WrongSizeException, IOException {
        // set input data using:
        // mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
        // mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (magneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
        final var nedFrame = new NEDFrame();
        final var earthB = new NEDMagneticFluxDensity();
        final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, BodyMagneticFluxDensity.COMPONENTS);
        final var y = new Matrix(numMeasurements, BodyMagneticFluxDensity.COMPONENTS);
        final var specificForceStandardDeviations = new double[numMeasurements];
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();

            x.setElementAt(i, 0, bTrueX);
            x.setElementAt(i, 1, bTrueY);
            x.setElementAt(i, 2, bTrueZ);

            y.setElementAt(i, 0, bMeasX);
            y.setElementAt(i, 1, bMeasY);
            y.setElementAt(i, 2, bMeasZ);

            specificForceStandardDeviations[i] = measurement.getMagneticFluxDensityStandardDeviation();
            i++;
        }

        fitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Converts magnetic flux density value and unit to Teslas.
     *
     * @param value magnetic flux density value.
     * @param unit  unit of magnetic flux density value.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
        return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Converts magnetic flux density instance to Teslas.
     *
     * @param magneticFluxDensity magnetic flux density instance to be converted.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
        return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4015
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4346
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2302
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1375
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1331
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 743
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1411
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1367
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2075
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2079
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3120
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1478
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3016
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2983
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 772
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 741
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1564
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1529
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1586
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1551
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 718
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2658
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1820
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2901
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1824
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2865
}

    /**
     * Gets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known x coordinate of gyroscope bias.
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known y coordinate of gyroscope bias.
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known z coordinate of gyroscope bias.
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known gyroscope bias coordinates expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @param biasY y coordinate of gyroscope bias.
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = biasX;
        this.biasY = biasY;
        this.biasZ = biasZ;
    }

    /**
     * Sets known gyroscope bias coordinates.
     *
     * @param biasX x coordinate of gyroscope bias.
     * @param biasY y coordinate of gyroscope bias.
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAngularSpeed(biasX);
        this.biasY = convertAngularSpeed(biasY);
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinate of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4015
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1375
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1331
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2075
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3158
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2464
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2079
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3120
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 728
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1478
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3016
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2983
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 772
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 741
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1564
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1529
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1586
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1551
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4015
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4346
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2302
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1363
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1375
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1331
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 743
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1411
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1367
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2075
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3158
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2464
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1382
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2079
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3120
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 728
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1478
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3016
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2983
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 772
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 741
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1564
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1529
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1586
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1551
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1364
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2114
}

    /**
     * Gets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2506
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2475
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return sequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setSequences(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.sequences = sequences;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return estimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public EasyGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4016
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4347
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2084
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2303
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1376
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1332
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 744
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1412
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1368
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2114
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2076
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3159
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2465
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2080
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3121
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 729
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1403
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1479
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3017
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2984
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 773
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 742
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1565
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1530
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1587
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1552
}

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4015
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4346
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2302
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1363
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1375
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1331
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 743
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1411
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1367
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2072
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2075
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3158
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2069
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1382
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2079
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3120
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3133
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3172
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1478
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3016
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 2983
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 772
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 741
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1564
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1529
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1586
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1551
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2073
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2114
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2070
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3134
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3173
}

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
        return initialSx;
    }

    /**
     * Sets initial x scaling factor of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSx(final double initialSx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
    }

    /**
     * Gets initial y scaling factor of gyroscope.
     *
     * @return initial y scaling factor of gyroscope.
     */
    @Override
    public double getInitialSy() {
        return initialSy;
    }

    /**
     * Sets initial y scaling factor of gyroscope.
     *
     * @param initialSy initial y scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSy(final double initialSy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSy = initialSy;
    }

    /**
     * Gets initial z scaling factor of gyroscope.
     *
     * @return initial z scaling factor of gyroscope.
     */
    @Override
    public double getInitialSz() {
        return initialSz;
    }

    /**
     * Sets initial z scaling factor of gyroscope.
     *
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialSz(final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSz = initialSz;
    }

    /**
     * Gets initial x-y cross coupling error of gyroscope.
     *
     * @return initial x-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxy() {
        return initialMxy;
    }

    /**
     * Sets initial x-y cross coupling error of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxy(final double initialMxy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
    }

    /**
     * Gets initial x-z cross coupling error of gyroscope.
     *
     * @return initial x-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMxz() {
        return initialMxz;
    }

    /**
     * Sets initial x-z cross coupling error of gyroscope.
     *
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMxz(final double initialMxz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxz = initialMxz;
    }

    /**
     * Gets initial y-x cross coupling error of gyroscope.
     *
     * @return initial y-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyx() {
        return initialMyx;
    }

    /**
     * Sets initial y-x cross coupling error of gyroscope.
     *
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyx(final double initialMyx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyx = initialMyx;
    }

    /**
     * Gets initial y-z cross coupling error of gyroscope.
     *
     * @return initial y-z cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMyz() {
        return initialMyz;
    }

    /**
     * Sets initial y-z cross coupling error of gyroscope.
     *
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMyz(final double initialMyz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMyz = initialMyz;
    }

    /**
     * Gets initial z-x cross coupling error of gyroscope.
     *
     * @return initial z-x cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzx() {
        return initialMzx;
    }

    /**
     * Sets initial z-x cross coupling error of gyroscope.
     *
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzx(final double initialMzx) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzx = initialMzx;
    }

    /**
     * Gets initial z-y cross coupling error of gyroscope.
     *
     * @return initial z-y cross coupling error of gyroscope.
     */
    @Override
    public double getInitialMzy() {
        return initialMzy;
    }

    /**
     * Sets initial z-y cross coupling error of gyroscope.
     *
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialMzy(final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors of gyroscope.
     *
     * @param initialSx initial x scaling factor of gyroscope.
     * @param initialSy initial y scaling factor of gyroscope.
     * @param initialSz initial z scaling factor of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactors(
            final double initialSx, final double initialSy, final double initialSz) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialSx = initialSx;
        this.initialSy = initialSy;
        this.initialSz = initialSz;
    }

    /**
     * Sets initial cross coupling errors of gyroscope.
     *
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialMxy = initialMxy;
        this.initialMxz = initialMxz;
        this.initialMyx = initialMyx;
        this.initialMyz = initialMyz;
        this.initialMzx = initialMzx;
        this.initialMzy = initialMzy;
    }

    /**
     * Sets initial scaling factors and cross coupling errors of
     * gyroscope.
     *
     * @param initialSx  initial x scaling factor of gyroscope.
     * @param initialSy  initial y scaling factor of gyroscope.
     * @param initialSz  initial z scaling factor of gyroscope.
     * @param initialMxy initial x-y cross coupling error of gyroscope.
     * @param initialMxz initial x-z cross coupling error of gyroscope.
     * @param initialMyx initial y-x cross coupling error of gyroscope.
     * @param initialMyz initial y-z cross coupling error of gyroscope.
     * @param initialMzx initial z-x cross coupling error of gyroscope.
     * @param initialMzy initial z-y cross coupling error of gyroscope.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        setInitialScalingFactors(initialSx, initialSy, initialSz);
        setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1957
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2066
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2024
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1443
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2171
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2305
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @return size of subsets to be checked during robust estimation.
     */
    public int getPreliminarySubsetSize() {
        return preliminarySubsetSize;
    }

    /**
     * Sets size of subsets to be checked during robust estimation.
     * This has to be at least {@link #MINIMUM_MEASUREMENTS}.
     *
     * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
     */
    public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2502
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2476
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {

        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return sequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setSequences(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.sequences = sequences;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Indicates whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @return true if G-dependent cross biases will be estimated,
     * false otherwise.
     */
    public boolean isGDependentCrossBiasesEstimated() {
        return estimateGDependentCrossBiases;
    }

    /**
     * Specifies whether G-dependent cross biases are being estimated
     * or not.
     * When enabled, this adds 9 variables from Gg matrix.
     *
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustEasyGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1744
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1791
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final var result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (position != null) {
            final var velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(position.getX(), position.getY(), position.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = convertPosition(position);
    }

    /**
     * Gets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @return list of body kinematics measurements.
     */
    @Override
    public List<StandardDeviationBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements list of body kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public AccelerometerCalibratorMeasurementType getMeasurementType() {
        return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndPositionAccelerometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2401
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2899
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(
            final Collection<? extends StandardDeviationFrameBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        //noinspection unchecked
        this.measurements = (Collection<StandardDeviationFrameBodyKinematics>) measurements;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS_MEASUREMENT;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mg matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mg matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    @Override
    public KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2039
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 469
this(measurements, commonAxisUsed, initialBias, initialMa);
        this.listener = listener;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
        return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasXAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasYAsAcceleration() {
        return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasYAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAcceleration(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasZAsAcceleration() {
        return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasZAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution
     * expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(
            final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(
            final Acceleration initialBiasX, final Acceleration initialBiasY, final Acceleration initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
        this.initialBiasY = convertAcceleration(initialBiasY);
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @return initial bias coordinates.
     */
    @Override
    public AccelerationTriad getInitialBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final AccelerationTriad initialBias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1835
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets ground truth gravity norm to be expected at location where measurements have been made,
     * expressed in meter per squared second (m/s^2).
     *
     * @return ground truth gravity norm or null.
     */
    public Double getGroundTruthGravityNorm() {
        return groundTruthGravityNorm;
    }

    /**
     * Sets ground truth gravity norm to be expected at location where
     * measurements have been made, expressed in meters per squared second
     * (m/s^2).
     *
     * @param groundTruthGravityNorm ground truth gravity norm or
     *                               null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthGravityNorm(final Double groundTruthGravityNorm) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
    }

    /**
     * Gets ground truth gravity norm to be expected at location where measurements have been made.
     *
     * @return ground truth gravity norm or null.
     */
    public Acceleration getGroundTruthGravityNormAsAcceleration() {
        return groundTruthGravityNorm != null
                ? new Acceleration(groundTruthGravityNorm, AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
    }

    /**
     * Gets ground truth gravity norm to be expected at location where measurements have been made.
     *
     * @param result instance where result will be stored.
     * @return true if ground truth gravity norm has been defined, false if it is not available yet.
     */
    public boolean getGroundTruthGravityNormAsAcceleration(final Acceleration result) {
        if (groundTruthGravityNorm != null) {
            result.setValue(groundTruthGravityNorm);
            result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets ground truth gravity norm to be expected at location where
     * measurements have been made.
     *
     * @param groundTruthGravityNorm ground truth gravity norm or null
     *                               if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthGravityNorm(final Acceleration groundTruthGravityNorm) throws LockedException {
        setGroundTruthGravityNorm(groundTruthGravityNorm != null ? convertAcceleration(groundTruthGravityNorm) : null);
    }

    /**
     * Gets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @return list of body kinematics measurements.
     */
    @Override
    public List<StandardDeviationBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements list of body kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public AccelerometerCalibratorMeasurementType getMeasurementType() {
        return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndGravityNormAccelerometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 848
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1931
return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for the accelerometer, gyroscope and magnetometer.
     *
     * @throws AlgebraException if there are numerical errors.
     * @throws IOException      if world magnetic model cannot be loaded.
     */
    private void calibrateCommonAxis() throws AlgebraException, IOException {
        // The magnetometer model is:
        // mBmeas = bm + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = bm + (I + Mm) * mBtrue

        // Hence:
        // [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        // [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        // [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        // where myx = mzx = mzy = 0

        // Hence:
        // [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        // [mBmeasy] = [by]     [0  1   0]   [0     sy  myz]    [mBtruey]
        // [mBmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [mBtruez]

        // [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        // [mBmeasy]   [by]     [0      1+sy    myz ][mBtruey]
        // [mBmeasz]   [bz]     [0      0       1+sz][mBtruez]

        // mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
        // mBmeasz = bz + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
        // Reordering:
        // mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
        // mBmeasz = bz + mBtruez + sz * mBtruez

        // mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
        // mBmeasz - mBtruez = bz + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruez][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0      ][bz ]   [mBmeasz - mBtruez]
        //                                                                   [sx ]
        //                                                                   [sy ]
        //                                                                   [sz ]
        //                                                                   [mxy]
        //                                                                   [mxz]
        //                                                                   [myz]

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (magneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
        final var nedFrame = new NEDFrame();
        final var earthB = new NEDMagneticFluxDensity();
        final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanInitializerConfig.java 112
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanInitializerConfig.java 132
public INSLooselyCoupledKalmanInitializerConfig(final INSLooselyCoupledKalmanInitializerConfig input) {
        copyFrom(input);
    }

    /**
     * Gets initial attitude uncertainty per axis expressed in radians (rad).
     *
     * @return initial attitude uncertainty per axis expressed in radians (rad).
     */
    public double getInitialAttitudeUncertainty() {
        return initialAttitudeUncertainty;
    }

    /**
     * Sets initial attitude uncertainty per axis expressed in radians (rad).
     *
     * @param initialAttitudeUncertainty initial attitude uncertainty per axis expressed
     *                                   in radians (rad).
     */
    public void setInitialAttitudeUncertainty(final double initialAttitudeUncertainty) {
        this.initialAttitudeUncertainty = initialAttitudeUncertainty;
    }

    /**
     * Gets initial attitude uncertainty per axis.
     *
     * @param result instance where initial attitude uncertainty per axis will be stored.
     */
    public void getInitialAttitudeUncertaintyAngle(final Angle result) {
        result.setValue(initialAttitudeUncertainty);
        result.setUnit(AngleUnit.RADIANS);
    }

    /**
     * Gets initial attitude uncertainty per axis.
     *
     * @return initial attitude uncertainty per axis.
     */
    public Angle getInitialAttitudeUncertaintyAngle() {
        return new Angle(initialAttitudeUncertainty, AngleUnit.RADIANS);
    }

    /**
     * Sets initial attitude uncertainty per axis.
     *
     * @param initialAttitudeUncertainty initial attitude uncertainty per axis.
     */
    public void setInitialAttitudeUncertainty(final Angle initialAttitudeUncertainty) {
        this.initialAttitudeUncertainty = AngleConverter.convert(initialAttitudeUncertainty.getValue().doubleValue(),
                initialAttitudeUncertainty.getUnit(), AngleUnit.RADIANS);
    }

    /**
     * Gets initial velocity uncertainty per axis expressed in meters per second (m/s).
     *
     * @return initial velocity uncertainty per axis expressed in meters per second (m/s).
     */
    public double getInitialVelocityUncertainty() {
        return initialVelocityUncertainty;
    }

    /**
     * Sets initial velocity uncertainty per axis expressed in meters per second (m/s).
     *
     * @param initialVelocityUncertainty initial velocity uncertainty per axis expressed
     *                                   in meters per second (m/s).
     */
    public void setInitialVelocityUncertainty(final double initialVelocityUncertainty) {
        this.initialVelocityUncertainty = initialVelocityUncertainty;
    }

    /**
     * Gets initial velocity uncertainty per axis.
     *
     * @param result instance where initial attitude uncertainty per axis will be stored.
     */
    public void getInitialVelocityUncertaintySpeed(final Speed result) {
        result.setValue(initialVelocityUncertainty);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets initial velocity uncertainty per axis.
     *
     * @return initial velocity uncertainty per axis.
     */
    public Speed getInitialVelocityUncertaintySpeed() {
        return new Speed(initialVelocityUncertainty, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets initial velocity uncertainty per axis.
     *
     * @param initialVelocityUncertainty initial velocity uncertainty per axis.
     */
    public void setInitialVelocityUncertainty(final Speed initialVelocityUncertainty) {
        this.initialVelocityUncertainty = SpeedConverter.convert(initialVelocityUncertainty.getValue().doubleValue(),
                initialVelocityUncertainty.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets initial position uncertainty per axis expressed in meters (m)
     *
     * @return initial position uncertainty per axis expressed in meters (m).
     */
    public double getInitialPositionUncertainty() {
        return initialPositionUncertainty;
    }

    /**
     * Sets initial position uncertainty per axis expressed in meters (m)
     *
     * @param initialPositionUncertainty initial position uncertainty per axis expressed
     *                                   in meters (m).
     */
    public void setInitialPositionUncertainty(final double initialPositionUncertainty) {
        this.initialPositionUncertainty = initialPositionUncertainty;
    }

    /**
     * Gets initial position uncertainty per axis.
     *
     * @param result instance where initial position uncertainty per axis will be stored.
     */
    public void getInitialPositionUncertaintyDistance(final Distance result) {
        result.setValue(initialPositionUncertainty);
        result.setUnit(DistanceUnit.METER);
    }

    /**
     * Gets initial position uncertainty per axis.
     *
     * @return initial position uncertainty per axis.
     */
    public Distance getInitialPositionUncertaintyDistance() {
        return new Distance(initialPositionUncertainty, DistanceUnit.METER);
    }

    /**
     * Sets initial position uncertainty per axis.
     *
     * @param initialPositionUncertainty initial position uncertainty per axis.
     */
    public void setInitialPositionUncertainty(final Distance initialPositionUncertainty) {
        this.initialPositionUncertainty = DistanceConverter.convert(initialPositionUncertainty.getValue().doubleValue(),
                initialPositionUncertainty.getUnit(), DistanceUnit.METER);
    }

    /**
     * Gets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
     *
     * @return initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
     */
    public double getInitialAccelerationBiasUncertainty() {
        return initialAccelerationBiasUncertainty;
    }

    /**
     * Sets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
     *
     * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty expressed in
     *                                           meters per squared second (m/s^2).
     */
    public void setInitialAccelerationBiasUncertainty(final double initialAccelerationBiasUncertainty) {
        this.initialAccelerationBiasUncertainty = initialAccelerationBiasUncertainty;
    }

    /**
     * Gets initial acceleration bias uncertainty.
     *
     * @param result instance where initial acceleration bias uncertainty will be stored.
     */
    public void getInitialAccelerationBiasUncertaintyAcceleration(final Acceleration result) {
        result.setValue(initialAccelerationBiasUncertainty);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial acceleration bias uncertainty.
     *
     * @return initial acceleration bias uncertainty.
     */
    public Acceleration getInitialAccelerationBiasUncertaintyAcceleration() {
        return new Acceleration(initialAccelerationBiasUncertainty, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial acceleration bias uncertainty.
     *
     * @param initialAccelerationUncertainty initial acceleration bias uncertainty.
     */
    public void setInitialAccelerationBiasUncertainty(final Acceleration initialAccelerationUncertainty) {
        initialAccelerationBiasUncertainty = AccelerationConverter.convert(
                initialAccelerationUncertainty.getValue().doubleValue(), initialAccelerationUncertainty.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias uncertainty expressed in radians per second (rad/s).
     */
    public double getInitialGyroscopeBiasUncertainty() {
        return initialGyroscopeBiasUncertainty;
    }

    /**
     * Sets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
     *
     * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty expressed
     *                                        in radians per second (rad/s).
     */
    public void setInitialGyroscopeBiasUncertainty(final double initialGyroscopeBiasUncertainty) {
        this.initialGyroscopeBiasUncertainty = initialGyroscopeBiasUncertainty;
    }

    /**
     * Gets initial gyroscope bias uncertainty.
     *
     * @param result instance where initial gyroscope bias uncertainty will be stored.
     */
    public void getInitialGyroscopeBiasUncertaintyAngularSpeed(final AngularSpeed result) {
        result.setValue(initialGyroscopeBiasUncertainty);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets initial gyroscope bias uncertainty.
     *
     * @return initial gyroscope bias uncertainty.
     */
    public AngularSpeed getInitialGyroscopeBiasUncertaintyAngularSpeed() {
        return new AngularSpeed(initialGyroscopeBiasUncertainty, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets initial gyroscope bias uncertainty.
     *
     * @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty.
     */
    public void setInitialGyroscopeBiasUncertainty(final AngularSpeed initialGyroscopeBiasUncertainty) {
        this.initialGyroscopeBiasUncertainty = AngularSpeedConverter.convert(
                initialGyroscopeBiasUncertainty.getValue().doubleValue(), initialGyroscopeBiasUncertainty.getUnit(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets configuration parameters.
     *
     * @param initialAttitudeUncertainty         initial attitude uncertainty per axis
     *                                           expressed in radians (rad).
     * @param initialVelocityUncertainty         initial velocity uncertainty per axis
     *                                           expressed in meters per second (m/s).
     * @param initialPositionUncertainty         initial position uncertainty per axis
     *                                           expressed in meters (m).
     * @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty
     *                                           expressed in meters per squared second (m/s^2).
     * @param initialGyroscopeBiasUncertainty    initial gyroscope bias uncertainty
     *                                           expressed in radians per second (rad/s).
     */
    public void setValues(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4086
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2041
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 471
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1138
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1094
}

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
        return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasXAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasYAsAcceleration() {
        return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasYAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = convertAcceleration(initialBiasY);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasZAsAcceleration() {
        return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getInitialBiasZAsAcceleration(final Acceleration result) {
        result.setValue(initialBiasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution
     * expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
        this.initialBiasY = initialBiasY;
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final Acceleration initialBiasX, final Acceleration initialBiasY,
                               final Acceleration initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = convertAcceleration(initialBiasX);
        this.initialBiasY = convertAcceleration(initialBiasY);
        this.initialBiasZ = convertAcceleration(initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @return initial bias coordinates.
     */
    @Override
    public AccelerationTriad getInitialBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
                initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getInitialBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets initial bias coordinates of accelerometer used to find a solution.
     *
     * @param initialBias initial bias coordinates to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBias(final AccelerationTriad initialBias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1184
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public List<StandardDeviationFrameBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationFrameBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS_MEASUREMENT;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndFrameGyroscopeCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2020
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2777
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4013
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1973
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1103
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldFrame.getCoordinateTransformation(),
                oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC,
                SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC,
                SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1374
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1435
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2476
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return sequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setSequences(final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.sequences = sequences;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2506
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2476
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return sequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setSequences(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.sequences = sequences;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2502
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
        return sequences;
    }

    /**
     * Sets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @param sequences collection of sequences of timestamped body
     *                  kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setSequences(final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.sequences = sequences;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return true;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1167
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 1313
}

    /**
     * Gets body to ECEF coordinate transformation.
     *
     * @param threshold threshold to determine whether current body to ECEF transformation
     *                  matrix is valid or not (to check that matrix is 3x3 orthonormal).
     * @return body to ECEF coordinate transformation.
     * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
     *                                        is considered not valid (is not a 3x3 orthonormal matrix) with provided threshold.
     */
    public CoordinateTransformation getC(final double threshold) throws InvalidRotationMatrixException {
        return bodyToEcefCoordinateTransformationMatrix != null
                ? new CoordinateTransformation(bodyToEcefCoordinateTransformationMatrix, FrameType.BODY_FRAME,
                FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME, threshold)
                : null;
    }

    /**
     * Gets body to ECEF coordinate transformation.
     *
     * @param result instance where body to ECEF coordinate transformation will be stored.
     * @return true if result instance was updated, false otherwise.
     * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
     *                                        is not valid (is not a 3x3 orthonormal matrix).
     */
    public boolean getC(final CoordinateTransformation result) throws InvalidRotationMatrixException {
        if (bodyToEcefCoordinateTransformationMatrix != null) {
            result.setSourceType(FrameType.BODY_FRAME);
            result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
            result.setMatrix(bodyToEcefCoordinateTransformationMatrix);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets body to ECEF coordinate transformation.
     *
     * @param result    instance where body to ECEF coordinate transformation will be stored.
     * @param threshold threshold to determine whether current body to ECEF transformation
     *                  matrix is valid or not (to check that matrix is 3x3 orthonormal).
     * @return true if result instance was updated, false otherwise.
     * @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
     *                                        is not valid (is not a 3x3 orthonormal matrix) with provided threshold.
     */
    public boolean getC(final CoordinateTransformation result, final double threshold)
            throws InvalidRotationMatrixException {
        if (bodyToEcefCoordinateTransformationMatrix != null) {
            result.setSourceType(FrameType.BODY_FRAME);
            result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
            result.setMatrix(bodyToEcefCoordinateTransformationMatrix, threshold);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets body to ECEF coordinate transformation.
     *
     * @param c body to ECEF coordinate transformation to be set.
     * @throws IllegalArgumentException if provided coordinate transformation is
     *                                  not null and is not a body to ECEF transformation.
     */
    public void setC(final CoordinateTransformation c) {
        if (c == null) {
            bodyToEcefCoordinateTransformationMatrix = null;

        } else {

            if (c.getSourceType() != FrameType.BODY_FRAME
                    || c.getDestinationType() != FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME) {
                throw new IllegalArgumentException();
            }

            if (bodyToEcefCoordinateTransformationMatrix != null) {
                c.getMatrix(bodyToEcefCoordinateTransformationMatrix);
            } else {
                bodyToEcefCoordinateTransformationMatrix = c.getMatrix();
            }
        }
    }

    /**
     * Gets estimated ECEF user velocity resolved around x axis.
     *
     * @param result instance where estimated ECEF user velocity resolved around x axis will be stored.
     */
    public void getSpeedX(final Speed result) {
        result.setValue(vx);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around x axis.
     *
     * @return estimated ECEF user velocity resolved around x axis.
     */
    public Speed getSpeedX() {
        return new Speed(vx, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity resolved around x axis.
     *
     * @param vx estimated ECEF user velocity resolved around x axis.
     */
    public void setSpeedX(final Speed vx) {
        this.vx = SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around y axis.
     *
     * @param result instance where estimated ECEF user velocity resolved around y axis will be stored.
     */
    public void getSpeedY(final Speed result) {
        result.setValue(vy);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around y axis.
     *
     * @return estimated ECEF velocity resolved around y axis.
     */
    public Speed getSpeedY() {
        return new Speed(vy, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity resolved around y axis.
     *
     * @param vy estimated ECEF user velocity resolved around y axis.
     */
    public void setSpeedY(final Speed vy) {
        this.vy = SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around z axis.
     *
     * @param result instance where estimated ECEF user velocity resolved around z axis will be stored.
     */
    public void getSpeedZ(final Speed result) {
        result.setValue(vz);
        result.setUnit(SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Gets estimated ECEF user velocity resolved around z axis.
     *
     * @return estimated ECEF velocity resolved around z axis.
     */
    public Speed getSpeedZ() {
        return new Speed(vz, SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity resolved around z axis.
     *
     * @param vz estimated ECEF velocity resolved around z axis.
     */
    public void setSpeedZ(final Speed vz) {
        this.vz = SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Sets estimated ECEF user velocity.
     *
     * @param vx estimated ECEF velocity resolved around x axis.
     * @param vy estimated ECEF velocity resolved around y axis.
     * @param vz estimated ECEF velocity resolved around z axis.
     */
    public void setVelocityCoordinates(final Speed vx, final Speed vy, final Speed vz) {
        setSpeedX(vx);
        setSpeedY(vy);
        setSpeedZ(vz);
    }

    /**
     * Gets estimated ECEF user velocity.
     *
     * @param result instance where estimated ECEF user velocity will be stored.
     */
    public void getEcefVelocity(final ECEFVelocity result) {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 764
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 878
public INSLooselyCoupledKalmanState(final INSLooselyCoupledKalmanState input) {
        copyFrom(input);
    }

    /**
     * Gets estimated body to ECEF coordinate transformation matrix.
     *
     * @return estimated body to ECEF coordinate transformation matrix.
     */
    public Matrix getBodyToEcefCoordinateTransformationMatrix() {
        return bodyToEcefCoordinateTransformationMatrix;
    }

    /**
     * Sets estimated body to ECEF coordinate transformation matrix.
     *
     * @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate
     *                                                 transformation matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    public void setBodyToEcefCoordinateTransformationMatrix(final Matrix bodyToEcefCoordinateTransformationMatrix) {
        if (bodyToEcefCoordinateTransformationMatrix.getRows() != CoordinateTransformation.ROWS
                || bodyToEcefCoordinateTransformationMatrix.getColumns() != CoordinateTransformation.COLS) {
            throw new IllegalArgumentException();
        }
        this.bodyToEcefCoordinateTransformationMatrix = bodyToEcefCoordinateTransformationMatrix;
    }

    /**
     * Gets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     *
     * @return estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     */
    public double getVx() {
        return vx;
    }

    /**
     * Sets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     *
     * @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     */
    public void setVx(final double vx) {
        this.vx = vx;
    }

    /**
     * Gets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     *
     * @return estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     */
    public double getVy() {
        return vy;
    }

    /**
     * Sets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     *
     * @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     */
    public void setVy(final double vy) {
        this.vy = vy;
    }

    /**
     * Gets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     *
     * @return estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     */
    public double getVz() {
        return vz;
    }

    /**
     * Sets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     *
     * @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     */
    public void setVz(final double vz) {
        this.vz = vz;
    }

    /**
     * Sets estimated ECEF user velocity coordinates.
     *
     * @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
     * @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
     * @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
     */
    public void setVelocityCoordinates(final double vx, final double vy, final double vz) {
        this.vx = vx;
        this.vy = vy;
        this.vz = vz;
    }

    /**
     * Gets x coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @return x coordinate of estimated ECEF user position expressed in meters (m).
     */
    public double getX() {
        return x;
    }

    /**
     * Sets x coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @param x x coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setX(final double x) {
        this.x = x;
    }

    /**
     * Gets y coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @return y coordinate of estimated ECEF user position expressed in meters (m).
     */
    public double getY() {
        return y;
    }

    /**
     * Sets y coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @param y y coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setY(final double y) {
        this.y = y;
    }

    /**
     * Gets z coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @return z coordinate of estimated ECEF user position expressed in meters (m).
     */
    public double getZ() {
        return z;
    }

    /**
     * Sets z coordinate of estimated ECEF user position expressed in meters (m).
     *
     * @param z z coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setZ(final double z) {
        this.z = z;
    }

    /**
     * Sets estimated ECEF user position coordinates.
     *
     * @param x x coordinate of estimated ECEF user position expressed in meters (m).
     * @param y y coordinate of estimated ECEF user position expressed in meters (m).
     * @param z z coordinate of estimated ECEF user position expressed in meters (m).
     */
    public void setPositionCoordinates(final double x, final double y, final double z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    /**
     * Gets estimated accelerometer bias resolved around x axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @return estimated accelerometer bias resolved around x axis and expressed in
     * meters per squared second (m/s^2).
     */
    public double getAccelerationBiasX() {
        return accelerationBiasX;
    }

    /**
     * Sets estimated accelerometer bias resolved around x axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
     *                          expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasX(final double accelerationBiasX) {
        this.accelerationBiasX = accelerationBiasX;
    }

    /**
     * Gets estimated accelerometer bias resolved around y axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @return estimated accelerometer bias resolved around y axis and expressed in
     * meters per squared second (m/s^2).
     */
    public double getAccelerationBiasY() {
        return accelerationBiasY;
    }

    /**
     * Sets estimated accelerometer bias resolved around y axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @param accelerationBiasY estimated accelerometer bias resolved around y axis
     *                          and expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasY(final double accelerationBiasY) {
        this.accelerationBiasY = accelerationBiasY;
    }

    /**
     * Gets estimated accelerometer bias resolved around z axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @return estimated accelerometer bias resolved around z axis and
     * expressed in meters per squared second (m/s^2).
     */
    public double getAccelerationBiasZ() {
        return accelerationBiasZ;
    }

    /**
     * Sets estimated accelerometer bias resolved around z axis and expressed in
     * meters per squared second (m/s^2).
     *
     * @param accelerationBiasZ estimated accelerometer bias resolved around z axis
     *                          and expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasZ(final double accelerationBiasZ) {
        this.accelerationBiasZ = accelerationBiasZ;
    }

    /**
     * Sets estimated accelerometer bias expressed in meters per squared second (m/s^2).
     *
     * @param accelerationBiasX estimated accelerometer bias resolved around x axis and
     *                          expressed in meters per squared second (m/s^2).
     * @param accelerationBiasY estimated accelerometer bias resolved around y axis and
     *                          expressed in meters per squared second (m/s^2).
     * @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
     *                          expressed in meters per squared second (m/s^2).
     */
    public void setAccelerationBiasCoordinates(
            final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ) {
        this.accelerationBiasX = accelerationBiasX;
        this.accelerationBiasY = accelerationBiasY;
        this.accelerationBiasZ = accelerationBiasZ;
    }

    /**
     * Gets estimated gyroscope bias resolved around x axis and expressed in
     * radians per second (rad/s).
     *
     * @return estimated gyroscope bias resolved around x axis and expressed in
     * radians per second (rad/s).
     */
    public double getGyroBiasX() {
        return gyroBiasX;
    }

    /**
     * Sets estimated gyroscope bias resolved around x axis and expressed in
     * radians per second (rad/s).
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasX(final double gyroBiasX) {
        this.gyroBiasX = gyroBiasX;
    }

    /**
     * Gets estimated gyroscope bias resolved around y axis and expressed in
     * radians per second (rad/s).
     *
     * @return estimated gyroscope bias resolved around y axis and expressed
     * in radians per second (rad/s).
     */
    public double getGyroBiasY() {
        return gyroBiasY;
    }

    /**
     * Sets estimated gyroscope bias resolved around y axis and expressed in
     * radians per second (rad/s).
     *
     * @param gyroBiasY estimated gyroscope bias resolved around y axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasY(final double gyroBiasY) {
        this.gyroBiasY = gyroBiasY;
    }

    /**
     * Gets estimated gyroscope bias resolved around z axis and expressed in
     * radians per second (rad/s).
     *
     * @return estimated gyroscope bias resolved around z axis and expressed
     * in radians per second (rad/s).
     */
    public double getGyroBiasZ() {
        return gyroBiasZ;
    }

    /**
     * Sets estimated gyroscope bias resolved around z axis and expressed in
     * radians per second (rad/s).
     *
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasZ(final double gyroBiasZ) {
        this.gyroBiasZ = gyroBiasZ;
    }

    /**
     * Sets estimated gyroscope bias coordinates expressed in radians
     * per second (rad/s).
     *
     * @param gyroBiasX estimated gyroscope bias resolved around x axis and
     *                  expressed in radians per second (rad/s).
     * @param gyroBiasY estimated gyroscope bias resolved around y axis and
     *                  expressed in radians per second (rad/s).
     * @param gyroBiasZ estimated gyroscope bias resolved around z axis and
     *                  expressed in radians per second (rad/s).
     */
    public void setGyroBiasCoordinates(final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ) {
        this.gyroBiasX = gyroBiasX;
        this.gyroBiasY = gyroBiasY;
        this.gyroBiasZ = gyroBiasZ;
    }

    /**
     * Gets Kalman filter error covariance matrix.
     * Notice that covariance is expressed in terms of ECEF coordinates.
     * If accuracy of position, attitude or velocity needs to be expressed in terms
     * of NED coordinates, their respective sub-matrices of this covariance matrix
     * must be rotated, taking into account the Jacobian of the matrix transformation
     * relating both coordinates, the covariance can be expressed following the law
     * of propagation of uncertainties
     * <a href="https://en.wikipedia.org/wiki/Propagation_of_uncertainty">(https://en.wikipedia.org/wiki/Propagation_of_uncertainty)</a>
     * as: cov(f(x)) = J*cov(x)*J'.
     *
     * @param result instance where result data will be copied to.
     * @return true if result data has been copied, false otherwise.
     */
    public boolean getCovariance(final Matrix result) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2499
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2495
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1177
public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2892
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1177
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3560
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3596
public void setInitialBias(final AngularSpeedTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2499
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2892
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2495
public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2499
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2495
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3560
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3596
public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
        initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
        initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanEpochEstimator.java 632
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java 112
INSLooselyCoupledKalmanState.NUM_PARAMS, INSLooselyCoupledKalmanState.NUM_PARAMS);

        final var tmp1 = omegaIe.multiplyByScalarAndReturnNew(propagationInterval);
        final var tmp2 = phiMatrix.getSubmatrix(0, 0, 2, 2);
        tmp2.subtract(tmp1);

        phiMatrix.setSubmatrix(0, 0, 2, 2, tmp2);

        final var estCbeOld = previousState.getBodyToEcefCoordinateTransformationMatrix();
        tmp1.copyFrom(estCbeOld);
        tmp1.multiplyByScalar(propagationInterval);

        phiMatrix.setSubmatrix(0, 12, 2, 14, tmp1);
        phiMatrix.setSubmatrix(3, 9, 5, 11, tmp1);

        final var measFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
        measFibb.setElementAtIndex(0, fx);
        measFibb.setElementAtIndex(1, fy);
        measFibb.setElementAtIndex(2, fz);

        estCbeOld.multiply(measFibb, tmp1);

        Utils.skewMatrix(tmp1, tmp2);
        tmp2.multiplyByScalar(-propagationInterval);

        phiMatrix.setSubmatrix(3, 0, 5, 2, tmp2);

        phiMatrix.getSubmatrix(3, 3, 5, 5, tmp1);
        tmp2.copyFrom(omegaIe);
        tmp2.multiplyByScalar(2.0 * propagationInterval);
        tmp1.subtract(tmp2);
        phiMatrix.setSubmatrix(3, 3, 5, 5, tmp1);

        final var sinPrevLat = Math.sin(previousLatitude);
        final var cosPrevLat = Math.cos(previousLatitude);
        final var sinPrevLat2 = sinPrevLat * sinPrevLat;
        final var cosPrevLat2 = cosPrevLat * cosPrevLat;

        // From (2.137)
        final var geocentricRadius = EARTH_EQUATORIAL_RADIUS_WGS84
                / Math.sqrt(1.0 - Math.pow(EARTH_ECCENTRICITY * sinPrevLat, 2.0)) * Math.sqrt(cosPrevLat2
                + Math.pow(1.0 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY, 2.0) * sinPrevLat2);

        final var prevX = previousState.getX();
        final var prevY = previousState.getY();
        final var prevZ = previousState.getZ();
        final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(prevX, prevY, prevZ);

        final var previousPositionNorm = Math.sqrt(prevX * prevX + prevY * prevY + prevZ * prevZ);

        final var estRebeOld = new Matrix(com.irurueta.navigation.frames.ECEFPosition.COMPONENTS, 1);
        estRebeOld.setElementAtIndex(0, prevX);
        estRebeOld.setElementAtIndex(1, prevY);
        estRebeOld.setElementAtIndex(2, prevZ);

        final var g = gravity.asMatrix();
        g.multiplyByScalar(-2.0 * propagationInterval / geocentricRadius);

        final var estRebeOldTrans = estRebeOld.transposeAndReturnNew();
        estRebeOldTrans.multiplyByScalar(1.0 / previousPositionNorm);

        g.multiply(estRebeOldTrans, tmp1);

        phiMatrix.setSubmatrix(3, 6, 5, 8, tmp1);

        for (var i = 0; i < com.irurueta.navigation.frames.ECEFPosition.COMPONENTS; i++) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3335
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 4124
if (estimatedMa == null) {
            estimatedMa = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedMa.initialize(0.0);
        }

        estimatedMa.setElementAt(0, 0, sx);
        estimatedMa.setElementAt(1, 0, myx);
        estimatedMa.setElementAt(2, 0, mzx);

        estimatedMa.setElementAt(0, 1, mxy);
        estimatedMa.setElementAt(1, 1, sy);
        estimatedMa.setElementAt(2, 1, mzy);

        estimatedMa.setElementAt(0, 2, mxz);
        estimatedMa.setElementAt(1, 2, myz);
        estimatedMa.setElementAt(2, 2, sz);

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     */
    private void setInputData() throws WrongSizeException {
        // set input data using:
        // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        final var expectedKinematics = new BodyKinematics();

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final var y = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final var specificForceStandardDeviations = new double[numMeasurements];
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            x.setElementAt(i, 0, fTrueX);
            x.setElementAt(i, 1, fTrueY);
            x.setElementAt(i, 2, fTrueZ);

            y.setElementAt(i, 0, fMeasX);
            y.setElementAt(i, 1, fMeasY);
            y.setElementAt(i, 2, fMeasZ);

            specificForceStandardDeviations[i] = measurement.getSpecificForceStandardDeviation();
            i++;
        }

        fitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Converts acceleration value and unit to meters per squared second.
     *
     * @param value acceleration value.
     * @param unit  unit of acceleration value.
     * @return converted value.
     */
    private static double convertAcceleration(final double value, final AccelerationUnit unit) {
        return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1364
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1328
final RobustKnownFrameMagnetometerCalibratorListener listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return magneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.magneticModel = magneticModel;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1155
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 690
running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateCommonAxis() throws AlgebraException {
        // The gyroscope model is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        // where myx = mzx = mzy = 0

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [0     sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [0     0      sz ]  [Ωtruez]   [g31   g32   g33][ftruez]


        // [Ωmeasx] = [bx] + ( [1+sx  mxy    mxz ]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0     1+sy   myz ]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0     0      1+sz]  [Ωtruez]   [g31   g32   g33][ftruez]

        // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23, g31, g32, g33
        // Reordering:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy - Ωtruey - by = sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz - Ωtruez - bz = sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // [Ωtruex  0       0       Ωtruey  Ωtruez  0       ftruex  ftruey  ftruez  0       0       0       0       0       0     ][sx ] =  [Ωmeasx - Ωtruex - bx]
        // [0       Ωtruey  0       0       0       Ωtruez  0       0       0       ftruex  ftruey  ftruez  0       0       0     ][sy ]    [Ωmeasy - Ωtruey - by]
        // [0       0       Ωtruez  0       0       0       0       0       0       0       0       0       ftruex  ftruey  ftruez][sz ]    [Ωmeasz - Ωtruez - bz]
        //                                                                                                                         [mxy]
        //                                                                                                                         [mxz]
        //                                                                                                                         [myz]
        //                                                                                                                         [g11]
        //                                                                                                                         [g12]
        //                                                                                                                         [g13]
        //                                                                                                                         [g21]
        //                                                                                                                         [g22]
        //                                                                                                                         [g23]
        //                                                                                                                         [g31]
        //                                                                                                                         [g32]
        //                                                                                                                         [g33]

        final var expectedKinematics = new BodyKinematics();

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5144
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4401
b.setElementAtIndex(2, bz);

            g.setElementAt(0, 0, g11);
            g.setElementAt(1, 0, g21);
            g.setElementAt(2, 0, g31);

            g.setElementAt(0, 1, g12);
            g.setElementAt(1, 1, g22);
            g.setElementAt(2, 1, g32);

            g.setElementAt(0, 2, g13);
            g.setElementAt(1, 2, g23);
            g.setElementAt(2, 2, g33);

            // fix kinematics
            final var numItems = measuredSequence.getItemsCount();
            final var measuredItems = measuredSequence.getSortedItems();
            final var fixedItems = fixedSequence.getSortedItems();
            for (var j = 0; j < numItems; j++) {
                final var measuredItem = measuredItems.get(j);
                final var fixedItem = fixedItems.get(j);

                fixKinematics(measuredItem.getKinematics(), fixedItem.getKinematics());
            }


            // integrate fixed sequence to obtain attitude change
            QuaternionIntegrator.integrateGyroSequence(fixedSequence, QuaternionStepIntegratorType.RUNGE_KUTTA, q);

            startPoint.setInhomogeneousCoordinates(point[0], point[1], point[2]);
            q.inverse();
            q.rotate(startPoint, endPoint);

            expectedEndPoint.setInhomogeneousCoordinates(point[3], point[4], point[5]);

            return expectedEndPoint.distanceTo(endPoint);

        } catch (final AlgebraException | RotationException e) {
            throw new EvaluationException(e);
        }
    }

    /**
     * Fixes provided kinematics with provided accelerometer parameters and
     * current gyroscope parameters.
     *
     * @param kinematics kinematics to be fixed with current values.
     * @param result     kinematics where result will be stored.
     * @throws AlgebraException if for some reason kinematics
     */
    private void fixKinematics(final BodyKinematics kinematics, final BodyKinematics result) throws AlgebraException {

        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
        // Ωmeas = M*(Ωtrue + b + G * ftrue)

        // M = I + Mg
        // bg = M*b --> b = M^-1*bg
        // Gg = M*G --> G = M^-1*Gg

        // Ωtrue = M^-1 * Ωmeas - b - G*ftrue

        // fix specific force
        measuredSpecificForce.setElementAtIndex(0, kinematics.getFx());
        measuredSpecificForce.setElementAtIndex(1, kinematics.getFy());
        measuredSpecificForce.setElementAtIndex(2, kinematics.getFz());

        accelerationFixer.fix(measuredSpecificForce, trueSpecificForce);

        // fix angular rate
        measuredAngularRate.setElementAtIndex(0, kinematics.getAngularRateX());
        measuredAngularRate.setElementAtIndex(1, kinematics.getAngularRateY());
        measuredAngularRate.setElementAtIndex(2, kinematics.getAngularRateZ());

        g.multiply(trueSpecificForce, tmp);
        invM.multiply(measuredAngularRate, trueAngularRate);
        trueAngularRate.subtract(b);
        trueAngularRate.subtract(tmp);

        result.setSpecificForceCoordinates(
                trueSpecificForce.getElementAtIndex(0),
                trueSpecificForce.getElementAtIndex(1),
                trueSpecificForce.getElementAtIndex(2));

        result.setAngularRateCoordinates(
                trueAngularRate.getElementAtIndex(0),
                trueAngularRate.getElementAtIndex(1),
                trueAngularRate.getElementAtIndex(2));
    }

    /**
     * Resets any previous estimations.
     */
    private void reset() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2477
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2699
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<? extends StandardDeviationFrameBodyKinematics> measurements)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        //noinspection unchecked
        this.measurements = (Collection<StandardDeviationFrameBodyKinematics>) measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public AccelerometerCalibratorMeasurementType getMeasurementType() {
        return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    @Override
    public KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3417
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3379
initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body magnetic flux density measurements
     *                     taken at different frames (positions, orientations
     *                     and velocities).
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<? extends StandardDeviationFrameBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        //noinspection unchecked
        this.measurements = (Collection<StandardDeviationFrameBodyMagneticFluxDensity>) measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public MagnetometerCalibratorMeasurementType getMeasurementType() {
        return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_MAGNETIC_FLUX_DENSITY;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    @Override
    public KnownFrameMagnetometerNonLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2506
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2502
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2476
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1184
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3556
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2476
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3515
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2506
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2401
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2899
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2502
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2476
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1184
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }

    /**
     * Gets collection of sequences of timestamped body kinematics
     * measurements taken at a given position where the device moves freely
     * with different orientations.
     *
     * @return collection of sequences of timestamped body kinematics
     * measurements.
     */
    @Override
    public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2506
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2401
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2475
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3556
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2899
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2502
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2476
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3515
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1184
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3567
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3603
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @return initial gyroscope scale factors and cross coupling errors
     * matrix.
     */
    @Override
    public Matrix getInitialMg() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMg(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope scale factors and cross coupling errors
     * matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial gyroscope scale factors and cross coupling errors matrix.
     *
     * @param initialMg initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMg(final Matrix initialMg) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMg.getElementAtIndex(0);
        initialMyx = initialMg.getElementAtIndex(1);
        initialMzx = initialMg.getElementAtIndex(2);

        initialMxy = initialMg.getElementAtIndex(3);
        initialSy = initialMg.getElementAtIndex(4);
        initialMzy = initialMg.getElementAtIndex(5);

        initialMxz = initialMg.getElementAtIndex(6);
        initialMyz = initialMg.getElementAtIndex(7);
        initialSz = initialMg.getElementAtIndex(8);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing initial g-dependent cross biases.
     */
    @Override
    public Matrix getInitialGg() {
        return new Matrix(initialGg);
    }

    /**
     * Gets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialGg(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        result.copyFrom(initialGg);
    }

    /**
     * Sets initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @param initialGg g-dependent cross biases.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void setInitialGg(final Matrix initialGg) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialGg.copyTo(this.initialGg);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1863
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1360
public void setListener(final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1927
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1421
public void setListener(final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1151
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 679
running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateCommonAxis() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        // [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // where myx = mzx = mzy = 0

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy] = [by]     [0  1   0]   [0     sy  myz]    [ftruey]
        // [fmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [ftruez]

        // [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        // [fmeasy]   [by]     [0      1+sy    myz ][ftruey]
        // [fmeasz]   [bz]     [0      0       1+sz][ftruez]

        // fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + (1+sy) * ftruey + myz * ftruez
        // fmeasz = bz + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz
        // Reordering:
        // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + ftruey + sy * ftruey + myz * ftruez
        // fmeasz = bz + ftruez + sz * ftruez

        // fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy - ftruey - by = sy * ftruey + myz * ftruez
        // fmeasz - ftruez - bz = sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruez][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0     ][sz ]   [fmeasz - ftruez - bz]
        //                                                 [mxy]
        //                                                 [mxz]
        //                                                 [myz]

        final var expectedKinematics = new BodyKinematics();

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1173
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1137
initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body magnetic flux density measurements
     *                     taken at different frames (positions, orientations
     *                     and velocities).
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setMeasurements(
            final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public MagnetometerCalibratorMeasurementType getMeasurementType() {
        return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_MAGNETIC_FLUX_DENSITY;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public RobustKnownFrameMagnetometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1959
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1987
hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public List<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
        return measurements;
    }

    /**
     * Sets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @param measurements collection of body magnetic flux density
     *                     measurements at a known position and timestamp
     *                     with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public MagnetometerCalibratorMeasurementType getMeasurementType() {
        return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2101
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2060
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2794
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4055
}

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2296
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1308
estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z) {
        final var result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z) {
        final var result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1671
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1167
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public List<StandardDeviationFrameBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationFrameBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public AccelerometerCalibratorMeasurementType getMeasurementType() {
        return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndFrameAccelerometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1005
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1117
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1073
internalSetBias(bias);
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(biasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known x coordinate of accelerometer bias.
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAcceleration(biasX);
    }

    /**
     * Gets known y coordinate of accelerometer bias.
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known y coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(biasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known y coordinate of accelerometer bias.
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = convertAcceleration(biasY);
    }

    /**
     * Gets known z coordinate of accelerometer bias.
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets known z coordinate of accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(biasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known z coordinate of accelerometer bias.
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known accelerometer bias coordinates expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @param biasY y coordinate of accelerometer bias.
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1819
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2900
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1031
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1823
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2864
accelerometerSz = accelerometerMa.getElementAtIndex(8);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return x-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return y-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasY y-coordinate of gyroscope known  bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @return z-coordinate of gyroscope known bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     * This is expressed in radians per second (rad/s).
     *
     * @param biasZ z-coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @return x-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets x-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets x-coordinate of gyroscope known bias.
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @return y-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets y-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets y-coordinate of gyroscope known bias.
     *
     * @param biasY y-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @return initial z-coordinate of gyroscope known bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets z-coordinate of gyroscope known bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets z-coordinate of gyroscope known bias.
     *
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known bias coordinates of gyroscope expressed in
     * radians per second (rad/s).
     *
     * @param biasX x-coordinate of gyroscope known bias.
     * @param biasY y-coordinate of gyroscope known bias.
     * @param biasZ z-coordinate of gyroscope known bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(
            final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 3758
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 709
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1824
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1006
}

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
        return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasXAsAcceleration(final Acceleration result) {
        result.setValue(biasX);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final Acceleration biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = convertAcceleration(biasX);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasYAsAcceleration() {
        return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasYAsAcceleration(final Acceleration result) {
        result.setValue(biasY);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final Acceleration biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = convertAcceleration(biasY);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasZAsAcceleration() {
        return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasZAsAcceleration(final Acceleration result) {
        result.setValue(biasZ);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets z-coordinate of known accelerometer bias to be used to find a solution.
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final Acceleration biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Sets known bias coordinates of accelerometer expressed in meters
     * per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @param biasY y-coordinate of known accelerometer bias.
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1453
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2804
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4065
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2111
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2070
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1514
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 718
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2658
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1032
}

    /**
     * Gets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of gyroscope bias expressed in radians per second
     * (rad/s).
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @return x coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedX() {
        return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known x coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedX(final AngularSpeed result) {
        result.setValue(biasX);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known x coordinate of gyroscope bias.
     *
     * @param biasX x coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final AngularSpeed biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasX = convertAngularSpeed(biasX);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @return y coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedY() {
        return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known y coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedY(final AngularSpeed result) {
        result.setValue(biasY);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known y coordinate of gyroscope bias.
     *
     * @param biasY y coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final AngularSpeed biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = convertAngularSpeed(biasY);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @return z coordinate of gyroscope bias.
     */
    @Override
    public AngularSpeed getBiasAngularSpeedZ() {
        return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets known z coordinate of gyroscope bias.
     *
     * @param result instance where result data will be stored.
     */
    @Override
    public void getBiasAngularSpeedZ(final AngularSpeed result) {
        result.setValue(biasZ);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known z coordinate of gyroscope bias.
     *
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Sets known gyroscope bias coordinates expressed in radians per second
     * (rad/s).
     *
     * @param biasX x coordinate of gyroscope bias.
     * @param biasY y coordinate of gyroscope bias.
     * @param biasZ z coordinate of gyroscope bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2245
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2273
public void setListener(final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
                && position != null && year != null;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return magneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.magneticModel = magneticModel;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 335
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 339
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 338
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1215
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1185
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return sequences.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(sequences.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustEasyGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1571
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1593
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        gravityNorm = computeGravityNorm();

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3396
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3459
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasTurntableGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1940
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1944
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1507
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 342
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 336
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1511
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 340
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 339
super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1878
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1941
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1878
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1435
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1374
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1941
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 336
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1511
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 340
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 339
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1941
super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1690
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3396
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3459
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1940
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1944
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10367
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10862
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener, DEFAULT_ROBUST_METHOD);
    }


    /**
     * Computes error of a preliminary result respect a given measurement.
     *
     * @param measurement       a measurement.
     * @param preliminaryResult a preliminary result.
     * @return computed error.
     */
    protected double computeError(
            final StandardDeviationBodyKinematics measurement, final PreliminaryResult preliminaryResult) {
        // We know that measured angular rate is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        final var measuredKinematics = measurement.getKinematics();

        final var specificForce = new double[]{
                measuredKinematics.getFx(),
                measuredKinematics.getFy(),
                measuredKinematics.getFz()
        };

        try {
            final var axis1 = ArrayUtils.normalizeAndReturnNew(specificForce);
            final var rot1 = new Quaternion(axis1, 0.0);

            final var nedC1 = new CoordinateTransformation(
                    rot1.asInhomogeneousMatrix(), FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);

            final var nedPosition = getNedPosition();
            final var nedFrame1 = new NEDFrame(nedPosition, nedC1);
            final var ecefFrame1 = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame1);
            var ti = this.timeInterval;
            var angleIncrement = turntableRotationRate * ti;
            if (Math.abs(angleIncrement) > Math.PI / 2.0) {
                // angle = rot_rate * interval
                // rot_rate * interval / x = angle / x

                // if we want angle / x = pi / 2, then:
                final var x = Math.abs(angleIncrement) / (Math.PI / 2.0);
                ti /= x;
                angleIncrement = turntableRotationRate * ti;
            }
            final var rot = new AxisRotation3D(axis1, angleIncrement);
            final var rot2 = rot1.combineAndReturnNew(rot);
            final var nedC2 = new CoordinateTransformation(rot2.asInhomogeneousMatrix(), FrameType.BODY_FRAME,
                    FrameType.LOCAL_NAVIGATION_FRAME);

            final var nedFrame2 = new NEDFrame(nedPosition, nedC2);
            final var ecefFrame2 = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame2);

            final var expectedKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(ti,
                    ecefFrame2, ecefFrame1);

            final var angularRateMeasX1 = measuredKinematics.getAngularRateX();
            final var angularRateMeasY1 = measuredKinematics.getAngularRateY();
            final var angularRateMeasZ1 = measuredKinematics.getAngularRateZ();

            final var angularRateTrueX = expectedKinematics.getAngularRateX();
            final var angularRateTrueY = expectedKinematics.getAngularRateY();
            final var angularRateTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            final var mg = preliminaryResult.estimatedMg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4743
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4037
for (final var sequence : sequences) {
            fixedSequences.add(new BodyKinematicsSequence<>(sequence));
        }

        accelerationFixer.setBias(ba);
        accelerationFixer.setCrossCouplingErrors(ma);

        var i = 0;
        for (final var sequence : sequences) {
            // sequence mean accelerometer samples of previous static
            // period will need to be fixed using accelerometer calibration
            // parameters
            measuredBeforeF[0] = sequence.getBeforeMeanFx();
            measuredBeforeF[1] = sequence.getBeforeMeanFy();
            measuredBeforeF[2] = sequence.getBeforeMeanFz();
            accelerationFixer.fix(measuredBeforeF, fixedBeforeF);

            measuredAfterF[0] = sequence.getAfterMeanFx();
            measuredAfterF[1] = sequence.getAfterMeanFy();
            measuredAfterF[2] = sequence.getAfterMeanFz();
            accelerationFixer.fix(measuredAfterF, fixedAfterF);

            // because we are only interested in gravity direction, we
            // normalize these vectors, so that gravity becomes independent
            // of current Earth position.
            ArrayUtils.normalize(fixedBeforeF);
            ArrayUtils.normalize(fixedAfterF);

            x.setSubmatrix(i, 0, i, 2, fixedBeforeF);
            x.setSubmatrix(i, 3, i, 5, fixedAfterF);

            y[i] = 0.0;

            standardDeviations[i] = computeAverageAngularRateStandardDeviation(sequence);
            i++;
        }

        fitter.setInputData(x, y, standardDeviations);
    }

    /**
     * Computes average angular rate standard deviation for measurements
     * in provided sequence.
     *
     * @param sequence a sequence.
     * @return average angular rate standard deviation expressed in radians
     * per second (rad/s).
     */
    private static double computeAverageAngularRateStandardDeviation(
            final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence) {
        final var items = sequence.getSortedItems();
        final var size = items.size();

        var result = 0.0;
        for (final var item : items) {
            result += item.getAngularRateStandardDeviation() / size;
        }

        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed value and unit to radians per second.
     *
     * @param value angular speed value.
     * @param unit  unit of angular speed value.
     * @return converted value.
     */
    private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
        return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }

    /**
     * Makes proper conversion of internal cross-coupling, bias and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param b internal bias matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b, final Matrix g) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2028
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2073
public void setListener(final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
                && groundTruthGravityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1987
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2034
public void setListener(final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurements() && position != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2133
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2161
public void setListener(final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
                && groundTruthMagneticFluxDensityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1396
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1457
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1422
}

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1900
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1963
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1386
}

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5871
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3007
setResult(crossCoupling, bias);

        // at this point covariance is expressed in terms of b and M, and must
        // be expressed in terms of ba and Ma.
        // We know that:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [m21 	m22 	m23]
        //     [m31 	m32 	m33]

        // and that ba and Ma are expressed as:
        // Ma = M - I
        // ba = M * b

        // Ma = [m11 - 1    m12         m13    ] =  [sx     mxy     mxz]
        //      [m21        m22 - 1     m23    ]    [myx    sy      myz]
        //      [m31        m32         m33 - 1]    [mzx    mzy     sz ]

        // ba = [m11 * bx + m12 * by + m13 * bz] = 	[bax]
        //      [m21 * bx + m22 * by + m23 * bz]	[bay]
        //      [m31 * bx + m32 * by + m33 * bz]	[baz]

        // Defining the linear application:
        // F(b, M) = F(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33)
        // as:
        // [bax] = 	[m11 * bx + m12 * by + m13 * bz]
        // [bay]	[m21 * bx + m22 * by + m23 * bz]
        // [baz]	[m31 * bx + m32 * by + m33 * bz]
        // [sx]		[m11 - 1]
        // [sy]		[m22 - 1]
        // [sz]		[m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [m21]
        // [myz]    [m23]
        // [mzx]    [m31]
        // [mzy]    [m32]

        // Then the Jacobian of F(b, M) is:
        // J = 	[m11  m12  m13  bx  0   0   by  0   0   bz  0   0 ]
        //	    [m21  m22  m23  0   bx  0   0   by  0   0   bz  0 ]
        //	    [m31  m32  m33  0   0   bx  0   0   by  0   0   bz]
        //	    [0    0    0    1   0   0   0   0   0   0   0   0 ]
        //	    [0    0    0    0   0   0   0   1   0   0   0   0 ]
        //	    [0    0    0    0   0   0   0   0   0   0   0   1 ]
        //	    [0    0    0    0   0   0   1   0   0   0   0   0 ]
        //	    [0    0    0    0   0   0   0   0   0   1   0   0 ]
        //	    [0    0    0    0   1   0   0   0   0   0   0   0 ]
        //	    [0    0    0    0   0   0   0   0   0   0   1   0 ]
        //	    [0    0    0    0   0   1   0   0   0   0   0   0 ]
        //	    [0    0    0    0   0   0   0   0   1   0   0   0 ]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS, GENERAL_UNKNOWNS);

        jacobian.setElementAt(0, 0, m11);
        jacobian.setElementAt(1, 0, m21);
        jacobian.setElementAt(2, 0, m31);

        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(1, 1, m22);
        jacobian.setElementAt(2, 1, m32);

        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(2, 2, m33);

        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(3, 3, 1.0);

        jacobian.setElementAt(1, 4, bx);
        jacobian.setElementAt(8, 4, 1.0);

        jacobian.setElementAt(2, 5, bx);
        jacobian.setElementAt(10, 5, 1.0);

        jacobian.setElementAt(0, 6, by);
        jacobian.setElementAt(6, 6, 1.0);

        jacobian.setElementAt(1, 7, by);
        jacobian.setElementAt(4, 7, 1.0);

        jacobian.setElementAt(2, 8, by);
        jacobian.setElementAt(11, 8, 1.0);

        jacobian.setElementAt(0, 9, bz);
        jacobian.setElementAt(7, 9, 1.0);

        jacobian.setElementAt(1, 10, bz);
        jacobian.setElementAt(9, 10, 1.0);

        jacobian.setElementAt(2, 11, bz);
        jacobian.setElementAt(5, 11, 1.0);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // accelerometer model can be better expressed as:
        // fmeas = T*K*(ftrue + b)
        // fmeas = M*(ftrue + b)
        // fmeas = M*ftrue + M*b

        // where:
        // M = I + Ma
        // ba = M*b = (I + Ma)*b --> b = M^-1*ba

        // We know that the norm of the true specific force is equal to the amount
        // of gravity at a certain Earth position
        // ||ftrue|| = ||g|| ~ 9.81 m/s^2

        // Hence:
        // fmeas - M*b = M*ftrue

        // M^-1 * (fmeas - M*b) = ftrue

        // ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
        // ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
        // ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
        // ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [0 		m22 	m23]
        //     [0 	 	0 		m33]


        final var gradientEstimator = new GradientEstimator(this::evaluateCommonAxis);

        final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1900
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1963
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1422
}

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1396
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1457
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1386
}

    /**
     * Indicates whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @return indicates whether a linear calibrator is used or not for
     * preliminary solutions.
     */
    public boolean isLinearCalibratorUsed() {
        return useLinearCalibrator;
    }

    /**
     * Specifies whether a linear calibrator is used or not for preliminary
     * solutions.
     *
     * @param linearCalibratorUsed indicates whether a linear calibrator is used
     *                             or not for preliminary solutions.
     * @throws LockedException if calibrator is currently running.
     */
    public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        useLinearCalibrator = linearCalibratorUsed;
    }

    /**
     * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @return true if preliminary solutions must be refined after an initial linear solution, false
     * otherwise.
     */
    public boolean isPreliminarySolutionRefined() {
        return refinePreliminarySolutions;
    }

    /**
     * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
     * If no initial solution is found using a linear solver, a non linear solver will be
     * used regardless of this value using an average solution as the initial value to be
     * refined.
     *
     * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
     *                                   initial linear solution, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        refinePreliminarySolutions = preliminarySolutionRefined;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6247
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3384
estimatedMa.setElementAt(i, i, estimatedMa.getElementAt(i, i) - 1.0);
        }

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 12.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];

        return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];

        final var m12 = params[4];
        final var m22 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        return evaluate(bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param bx  x-coordinate of bias.
     * @param by  y-coordinate of bias.
     * @param bz  z-coordinate of bias.
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double bx, final double by, final double bz,
                            final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33) throws EvaluationException {

        // fmeas = M*(ftrue + b)

        // ftrue = M^-1*fmeas - b

        try {
            if (fmeas == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 973
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 977
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2917
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4302
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 973
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2370
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1683
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1637
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 977
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2917
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2363
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3446
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3408
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 715
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 387
estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final Point3D position, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1782
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3271
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1029
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1852
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1817
}

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getHardIron() {
        final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
        getHardIron(result);
        return result;
    }

    /**
     * Gets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    @Override
    public void getHardIron(final double[] result) {
        if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = hardIronX;
        result[1] = hardIronY;
        result[2] = hardIronZ;
    }

    /**
     * Sets known hard-iron bias as an array.
     * Array values are expressed in Teslas (T).
     *
     * @param hardIron known hard-iron.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setHardIron(final double[] hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        hardIronX = hardIron[0];
        hardIronY = hardIron[1];
        hardIronZ = hardIron[2];
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @return known hard-iron bias as a column matrix.
     */
    @Override
    public Matrix getHardIronMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            getHardIronMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known hard-iron bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getHardIronMatrix(final Matrix result) {
        if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, hardIronX);
        result.setElementAtIndex(1, hardIronY);
        result.setElementAtIndex(2, hardIronZ);
    }

    /**
     * Sets known hard-iron bias as a column matrix.
     *
     * @param hardIron known hard-iron bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setHardIron(final Matrix hardIron) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        hardIronX = hardIron.getElementAtIndex(0);
        hardIronY = hardIron.getElementAtIndex(1);
        hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2371
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3433
final var result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @return initial bias coordinates.
     */
    public AngularSpeedTriad getInitialBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialBiasAsTriad(AngularSpeedTriad result) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2368
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3471
final double[] result = new double[BodyKinematics.COMPONENTS];
        getInitialBias(result);
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void getInitialBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = initialBiasX;
        result[1] = initialBiasY;
        result[2] = initialBiasZ;
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param initialBias initial bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    public void setInitialBias(final double[] initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (initialBias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        initialBiasX = initialBias[0];
        initialBiasY = initialBias[1];
        initialBiasZ = initialBias[2];
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @return initial gyroscope bias to be used to find a solution as a
     * column matrix.
     */
    public Matrix getInitialBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getInitialBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial gyroscope bias to be used to find a solution as a
     * column matrix.
     * Values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void getInitialBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialBiasX);
        result.setElementAtIndex(1, initialBiasY);
        result.setElementAtIndex(2, initialBiasZ);
    }

    /**
     * Sets initial gyroscope bias to be used to find a solution as
     * a column matrix with values expressed in radians per second (rad/s).
     *
     * @param initialBias initial gyroscope bias to find a solution.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setInitialBias(final Matrix initialBias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        initialBiasX = initialBias.getElementAtIndex(0);
        initialBiasY = initialBias.getElementAtIndex(1);
        initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @return initial bias coordinates.
     */
    public AngularSpeedTriad getInitialBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
    }

    /**
     * Gets initial bias coordinates of gyroscope used to find a solution.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialBiasAsTriad(AngularSpeedTriad result) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1127
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2211
fillHardIronBiases(bx, by, bz);
        fillMm(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     * @throws IOException      if world magnetic model cannot be loaded.
     */
    private void calibrateGeneral() throws AlgebraException, IOException {
        // The magnetometer model is:
        // mBmeas = bm + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = bm + (I + Mm) * mBtrue

        // Hence:
        // [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        // [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        // [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        // [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        // [mBmeasy]   [by]     [myx    1+sy    myz ][mBtruey]
        // [mBmeasz]   [bz]     [mzx    mzy     1+sz][mBtruez]

        // mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + myx * mBtruex + (1+sy) * mBtruey + myz * mBtruez
        // mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        // mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
        // mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez

        // mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy - mBtruey = by + myx * mBtruex + sy * mBtruey + myz * mBtruez
        // mBmeasz - mBtruez = bz + mzx * mBtruex + mzy * mBtruey + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0        0        0        0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruex  mBtruez  0        0      ][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0        0        mBtruex  mBtruey][bz ]   [mBmeasz - mBtruez]
        //                                                                                              [sx ]
        //                                                                                              [sy ]
        //                                                                                              [sz ]
        //                                                                                              [mxy]
        //                                                                                              [mxz]
        //                                                                                              [myx]
        //                                                                                              [myz]
        //                                                                                              [mzx]
        //                                                                                              [mzy]

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (magneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
        final var nedFrame = new NEDFrame();
        final var earthB = new NEDMagneticFluxDensity();
        final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, GENERAL_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1219
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1189
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return qualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return stopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return sequences.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return preliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                        return computeError(sequences.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustEasyGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 343
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 343
final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return qualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return stopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return measurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return preliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                        return computeError(measurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownFrameGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1576
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1594
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        gravityNorm = computeGravityNorm();

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3891
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5240
return evaluateGeneral(i, params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
        final var m21 = result[1];
        final var m31 = result[2];

        final var m12 = result[3];
        final var m22 = result[4];
        final var m32 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, m21);
        mm.setElementAtIndex(2, m31);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, m32);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        setResult(mm);

        // at this point covariance is expressed in terms of M, and must
        // be expressed in terms of Mg.
        // We know that:
        // Mg = M - I
        // Gg = M * G

        // M = [m11  m12  m13]
        //     [m21  m22  m23]
        //     [m31  m32  m33]

        // G = [g11  g12  g13] = 0
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [m21        m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [m31        m32         m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [m21  m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [m31  m32  m33][g31  g32  g33]

        // Defining the linear application:
        // F(M) = F(m11, m21, m31, m12, m22, m32, m13, m23, m33)
        // as:
        // [sx] =   [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [m21]
        // [myz]    [m23]
        // [mzx]    [m31]
        // [mzy]    [m32]
        // [gg11]   [0]
        // [gg21]   [0]
        // [gg31]   [0]
        // [gg12]   [0]
        // [gg22]   [0]
        // [gg32]   [0]
        // [gg13]   [0]
        // [gg23]   [0]
        // [gg33]   [0]

        // Then the Jacobian of F(M) is:
        // J = [1    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    1    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    1]
        //     [0    0    0    1    0    0    0    0    0]
        //     [0    0    0    0    0    0    1    0    0]
        //     [0    1    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    1    0]
        //     [0    0    1    0    0    0    0    0    0]
        //     [0    0    0    0    0    1    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]
        //     [0    0    0    0    0    0    0    0    0]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS);

        jacobian.setElementAt(0, 0, 1.0);
        jacobian.setElementAt(1, 4, 1.0);
        jacobian.setElementAt(2, 8, 1.0);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 6, 1.0);
        jacobian.setElementAt(5, 1, 1.0);

        jacobian.setElementAt(6, 7, 1.0);
        jacobian.setElementAt(7, 2, 1.0);
        jacobian.setElementAt(8, 5, 1.9);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws AlgebraException if there are numerical instabilities.
     */
    private void setInputData() throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2781
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2754
return EasyGyroscopeCalibrator.MINIMUM_SEQUENCES_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3990
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4042
return KnownBiasTurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1149
}

    /**
     * Fills scale factor and cross coupling error matrix with estimated values.
     *
     * @param sx  x scale factor
     * @param sy  y scale factor
     * @param sz  z scale factor
     * @param mxy x-y cross coupling
     * @param mxz x-z cross coupling
     * @param myx y-x cross coupling
     * @param myz y-z cross coupling
     * @param mzx z-x cross coupling
     * @param mzy z-y cross coupling
     * @throws WrongSizeException never happens.
     */
    private void fillMg(final double sx, final double sy, final double sz,
                        final double mxy, final double mxz, final double myx,
                        final double myz, final double mzx, final double mzy) throws WrongSizeException {
        if (estimatedMg == null) {
            estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        }

        estimatedMg.setElementAt(0, 0, sx);
        estimatedMg.setElementAt(1, 0, myx);
        estimatedMg.setElementAt(2, 0, mzx);

        estimatedMg.setElementAt(0, 1, mxy);
        estimatedMg.setElementAt(1, 1, sy);
        estimatedMg.setElementAt(2, 1, mzy);

        estimatedMg.setElementAt(0, 2, mxz);
        estimatedMg.setElementAt(1, 2, myz);
        estimatedMg.setElementAt(2, 2, sz);
    }

    /**
     * Fills G-dependant cross biases.
     *
     * @param g11 element 1,1 of G-dependant cross biases matrix.
     * @param g12 element 1,2 of G-dependant cross biases matrix.
     * @param g13 element 1,3 of G-dependant cross biases matrix.
     * @param g21 element 2,1 of G-dependant cross biases matrix.
     * @param g22 element 2,2 of G-dependant cross biases matrix.
     * @param g23 element 2,3 of G-dependant cross biases matrix.
     * @param g31 element 3,1 of G-dependant cross biases matrix.
     * @param g32 element 3,2 of G-dependant cross biases matrix.
     * @param g33 element 3,3 of G-dependant cross biases matrix.
     * @throws WrongSizeException never happens.
     */
    private void fillGg(final double g11, final double g12, final double g13,
                        final double g21, final double g22, final double g23,
                        final double g31, final double g32, final double g33) throws WrongSizeException {
        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        }

        estimatedGg.setElementAt(0, 0, g11);
        estimatedGg.setElementAt(0, 1, g12);
        estimatedGg.setElementAt(0, 2, g13);

        estimatedGg.setElementAt(1, 0, g21);
        estimatedGg.setElementAt(1, 1, g22);
        estimatedGg.setElementAt(1, 2, g23);

        estimatedGg.setElementAt(2, 0, g31);
        estimatedGg.setElementAt(2, 1, g32);
        estimatedGg.setElementAt(2, 2, g33);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4595
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5977
setResult(m, b);

        // at this point covariance is expressed in terms of b, M and G, and must
        // be expressed in terms of bg, Mg and Gg.
        // We know that:
        // bg = M * b
        // Mg = M - I
        // Gg = M * G = 0

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11  m12  m13]
        //     [m21  m22  m23]
        //     [m31  m32  m33]

        // G = [g11  g12  g13] = 0
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // bg = [m11  m12  m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
        //      [m21  m22  m23][by]   [m21 * bx + m22 * by + m23 * bz]   [bgy]
        //      [m31  m32  m33][bz]   [m31 * bx + m32 * by + m33 * bz]   [bgz]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [m21        m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [m31        m32         m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = 0
        //      [gg21  gg22  gg23]
        //      [gg31  gg32  gg33]

        // Defining the linear application:
        // F(b, M) = F(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33)
        // as:
        // [bgx] =  [m11 * bx + m12 * by + m13 * bz]
        // [bgy]    [m21 * bx + m22 * by + m23 * bz]
        // [bgz]    [m31 * bx + m32 * by + m33 * bz]
        // [sx]     [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [m21]
        // [myz]    [m23]
        // [mzx]    [m31]
        // [mzy]    [m32]
        // [gg11]   [0]
        // [gg21]   [0]
        // [gg31]   [0]
        // [gg12]   [0]
        // [gg22]   [0]
        // [gg32]   [0]
        // [gg13]   [0]
        // [gg23]   [0]
        // [gg33]   [0]

        // Then the Jacobian of F(b, M) is:
        // J = [m11  m12  m13  bx   0    0    by   0    0    bz   0    0 ]
        //     [m21  m22  m23  0    bx   0    0    by   0    0    bz   0 ]
        //     [m31  m32  m33  0    0    bx   0    0    by   0    0    bz]
        //     [0    0    0    1    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    1    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    1 ]
        //     [0    0    0    0    0    0    1    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    1    0    0 ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    1    0 ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    1    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0 ]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS);

        jacobian.setElementAt(0, 0, m11);
        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(0, 6, by);
        jacobian.setElementAt(0, 9, bz);

        jacobian.setElementAt(1, 0, m21);
        jacobian.setElementAt(1, 1, m22);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(1, 4, bx);
        jacobian.setElementAt(1, 7, by);
        jacobian.setElementAt(1, 10, bz);

        jacobian.setElementAt(2, 0, m31);
        jacobian.setElementAt(2, 1, m32);
        jacobian.setElementAt(2, 2, m33);
        jacobian.setElementAt(2, 5, bx);
        jacobian.setElementAt(2, 8, by);
        jacobian.setElementAt(2, 11, bz);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 7, 1.0);
        jacobian.setElementAt(5, 11, 1.0);

        jacobian.setElementAt(6, 6, 1.0);
        jacobian.setElementAt(7, 9, 1.0);
        jacobian.setElementAt(8, 4, 1.0);

        jacobian.setElementAt(9, 10, 1.0);
        jacobian.setElementAt(10, 5, 1.0);
        jacobian.setElementAt(11, 8, 1.9);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws AlgebraException if there are numerical instabilities.
     */
    private void setInputData() throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3399
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3464
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return qualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return stopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return measurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return preliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                        return computeError(measurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1943
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1948
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return qualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return stopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return measurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return preliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                        return computeError(measurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2169
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1237
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz, final Point3D position,
            final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz, final Point3D position,
            final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
                position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 346
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 344
super(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return qualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return stopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return measurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return preliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                        return computeError(measurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
super(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
                    @Override
                    public double[] getQualityScores() {
                        return qualityScores;
                    }

                    @Override
                    public double getThreshold() {
                        return stopThreshold;
                    }

                    @Override
                    public int getTotalSamples() {
                        return measurements.size();
                    }

                    @Override
                    public int getSubsetSize() {
                        return preliminarySubsetSize;
                    }

                    @Override
                    public void estimatePreliminarSolutions(
                            final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                        computePreliminarySolutions(samplesIndices, solutions);
                    }

                    @Override
                    public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                        return computeError(measurements.get(i), currentEstimation);
                    }

                    @Override
                    public boolean isReady() {
                        return PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2834
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4110
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed
     * for both the accelerometer and gyroscope and when G-dependent cross
     * biases are being estimated.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxisAndGDependentCrossBiases() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6252
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6423
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3389
estimatedMse = fitter.getMse();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 12.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];

        return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];

        final var m12 = params[4];
        final var m22 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        return evaluate(bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param bx  x-coordinate of bias.
     * @param by  y-coordinate of bias.
     * @param bz  z-coordinate of bias.
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double bx, final double by, final double bz,
                            final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33) throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1676
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1695
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1676
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1695
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3399
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3464
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1943
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1948
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 184
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 189
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 186
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3092
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2834
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 596
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 580
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return sequences.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(sequences.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 759
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 768
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        gravityNorm = computeGravityNorm();

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4664
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2752
listener.onCalibrateEnd((C) this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5794
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5316
angularRateFixer.setBias(preliminaryResult.estimatedBiases);
            angularRateFixer.setCrossCouplingErrors(preliminaryResult.estimatedMg);
            angularRateFixer.setGDependantCrossBias(preliminaryResult.estimatedGg);

            // copy measured sequence as it will be used to fix kinematics values
            // using preliminary gyroscope parameters
            final var fixedSequence = new BodyKinematicsSequence<>(sequence);

            // fix body kinematic measurements of provided sequence
            final var numItems = sequence.getItemsCount();
            final var measuredItems = sequence.getSortedItems();
            final var fixedItems = fixedSequence.getSortedItems();
            for (var j = 0; j < numItems; j++) {
                final var measuredItem = measuredItems.get(j);
                final var fixedItem = fixedItems.get(j);

                final var measuredKinematics = measuredItem.getKinematics();
                final var fixedKinematics = fixedItem.getKinematics();
                fixKinematics(measuredKinematics, fixedKinematics);
            }

            // integrate fixed sequence to obtain attitude change
            QuaternionIntegrator.integrateGyroSequence(fixedSequence, QuaternionStepIntegratorType.RUNGE_KUTTA, q);

            // fix before coordinates
            measuredSpecificForce[0] = sequence.getBeforeMeanFx();
            measuredSpecificForce[1] = sequence.getBeforeMeanFy();
            measuredSpecificForce[2] = sequence.getBeforeMeanFz();
            accelerationFixer.fix(measuredSpecificForce, fixedSpecificForce);

            // normalize coordinates
            ArrayUtils.normalize(fixedSpecificForce);

            // compute estimated normalized end coordinates
            startPoint.setCoordinates(fixedSpecificForce);
            q.inverse();
            q.rotate(startPoint, endPoint);

            // fix after coordinates
            measuredSpecificForce[0] = sequence.getAfterMeanFx();
            measuredSpecificForce[1] = sequence.getAfterMeanFy();
            measuredSpecificForce[2] = sequence.getAfterMeanFz();
            accelerationFixer.fix(measuredSpecificForce, fixedSpecificForce);

            // normalize coordinates
            ArrayUtils.normalize(fixedSpecificForce);

            expectedEndPoint.setCoordinates(fixedSpecificForce);

            // compare estimated normalized end coordinates with expected
            // ones
            return expectedEndPoint.distanceTo(endPoint);

        } catch (final AlgebraException | RotationException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(
            final int[] samplesIndices, final List<PreliminaryResult> solutions) {
        final var seqs = new ArrayList<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>>();

        for (final var samplesIndex : samplesIndices) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4409
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2699
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4743
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2477
initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1797
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3417
hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1879
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3379
initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1173
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1959
initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1987
hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets a list of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5217
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 5715
this(convertPosition(position), measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }

        this.position = position;
        if (position != null) {
            final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
                    this.position.getX(), this.position.getY(), this.position.getZ());
            groundTruthGravityNorm = gravity.getNorm();
        } else {
            groundTruthGravityNorm = null;
        }
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final var result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (position != null) {
            final var velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    position.getX(), position.getY(), position.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        setPosition(position != null ? convertPosition(position) : null);
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && position != null;
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final var velocity = new ECEFVelocity();
        final var result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1641
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1673
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2793
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4002
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2766
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4054
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4409
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2477
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1167
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 704
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 189
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 814
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 823
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 185
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 708
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 190
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 187
super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1797
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3379
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1173
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1987
hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1879
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3417
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1959
initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 704
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 909
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 814
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 185
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 823
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 708
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 190
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 187
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1791
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets ground truth gravity norm to be expected at location where measurements have been made,
     * expressed in meter per squared second (m/s^2).
     *
     * @return ground truth gravity norm or null.
     */
    public Double getGroundTruthGravityNorm() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1744
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1835
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2100
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2059
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2189
&& groundTruthGravityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3092
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4110
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4409
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2477
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1835
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1791
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4743
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2699
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1744
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1167
initialBiasZ = initialBias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1797
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3379
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1959
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1952
hardIronZ = hardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1879
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3417
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1173
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1924
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1987
initialHardIronZ = initialHardIron.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMm() {
        Matrix result;
        try {
            result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            getInitialMm(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMm(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMm initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMm(final Matrix initialMm) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMm.getElementAtIndex(0);
        initialMyx = initialMm.getElementAtIndex(1);
        initialMzx = initialMm.getElementAtIndex(2);

        initialMxy = initialMm.getElementAtIndex(3);
        initialSy = initialMm.getElementAtIndex(4);
        initialMzy = initialMm.getElementAtIndex(5);

        initialMxz = initialMm.getElementAtIndex(6);
        initialMyz = initialMm.getElementAtIndex(7);
        initialSz = initialMm.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4409
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4743
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2477
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2699
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1671
biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }

    /**
     * Gets a collection of body kinematics measurements taken at
     * a given position with different unknown orientations and containing
     * the standard deviations of accelerometer and gyroscope measurements.
     *
     * @return collection of body kinematics measurements at a known position
     * with unknown orientations.
     */
    @Override
    public Collection<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2055
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2013
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2160
&& groundTruthGravityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2794
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4055
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2190
}

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each sequence.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2055
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2059
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2189
&& groundTruthGravityNorm != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2013
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2100
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2160
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements() && position != null;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2056
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2014
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2767
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4003
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2161
}

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1671
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1744
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1835
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1791
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    @Override
    public Matrix getInitialMa() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            getInitialMa(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param result instance where data will be stored.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     */
    @Override
    public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        initialSx = initialMa.getElementAtIndex(0);
        initialMyx = initialMa.getElementAtIndex(1);
        initialMzx = initialMa.getElementAtIndex(2);

        initialMxy = initialMa.getElementAtIndex(3);
        initialSy = initialMa.getElementAtIndex(4);
        initialMzy = initialMa.getElementAtIndex(5);

        initialMxz = initialMa.getElementAtIndex(6);
        initialMyz = initialMa.getElementAtIndex(7);
        initialSz = initialMa.getElementAtIndex(8);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2056
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2014
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2794
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4055
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2161
}

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2101
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2060
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2767
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4003
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2190
}

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1453
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1514
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2200
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2333
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2111
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2070
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2804
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4065
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1479
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2059
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3681
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2918
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3166
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1957
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2777
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4013
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2171
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2305
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2066
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2024
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2020
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1443
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1957
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2111
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2070
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2804
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2020
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4065
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1443
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2200
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2333
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2066
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2024
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1453
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2777
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4013
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1514
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1479
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2171
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2305
}

    /**
     * Returns amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @return amount of progress variation before notifying a progress change during
     * calibration.
     */
    public float getProgressDelta() {
        return progressDelta;
    }

    /**
     * Sets amount of progress variation before notifying a progress change during
     * calibration.
     *
     * @param progressDelta amount of progress variation before notifying a progress
     *                      change during calibration.
     * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setProgressDelta(final float progressDelta) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
            throw new IllegalArgumentException();
        }
        this.progressDelta = progressDelta;
    }

    /**
     * Returns amount of confidence expressed as a value between 0.0 and 1.0
     * (which is equivalent to 100%). The amount of confidence indicates the probability
     * that the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @return amount of confidence as a value between 0.0 and 1.0.
     */
    public double getConfidence() {
        return confidence;
    }

    /**
     * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
     * equivalent to 100%). The amount of confidence indicates the probability that
     * the estimated result is correct. Usually this value will be close to 1.0, but
     * not exactly 1.0.
     *
     * @param confidence confidence to be set as a value between 0.0 and 1.0.
     * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setConfidence(final double confidence) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
            throw new IllegalArgumentException();
        }
        this.confidence = confidence;
    }

    /**
     * Returns maximum allowed number of iterations. If maximum allowed number of
     * iterations is achieved without converging to a result when calling calibrate(),
     * a RobustEstimatorException will be raised.
     *
     * @return maximum allowed number of iterations.
     */
    public int getMaxIterations() {
        return maxIterations;
    }

    /**
     * Sets maximum allowed number of iterations. When the maximum number of iterations
     * is exceeded, result will not be available, however an approximate result will be
     * available for retrieval.
     *
     * @param maxIterations maximum allowed number of iterations to be set.
     * @throws IllegalArgumentException if provided value is less than 1.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setMaxIterations(final int maxIterations) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (maxIterations < MIN_ITERATIONS) {
            throw new IllegalArgumentException();
        }
        this.maxIterations = maxIterations;
    }

    /**
     * Gets data related to inliers found after estimation.
     *
     * @return data related to inliers found after estimation.
     */
    public InliersData getInliersData() {
        return inliersData;
    }

    /**
     * Indicates whether result must be refined using a non-linear solver over found inliers.
     *
     * @return true to refine result, false to simply use result found by robust estimator
     * without further refining.
     */
    public boolean isResultRefined() {
        return refineResult;
    }

    /**
     * Specifies whether result must be refined using a non-linear solver over found inliers.
     *
     * @param refineResult true to refine result, false to simply use result found by robust
     *                     estimator without further refining.
     * @throws LockedException if calibrator is currently running.
     */
    public void setResultRefined(final boolean refineResult) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.refineResult = refineResult;
    }

    /**
     * Indicates whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @return true if covariance must be kept after refining result, false otherwise.
     */
    public boolean isCovarianceKept() {
        return keepCovariance;
    }

    /**
     * Specifies whether covariance must be kept after refining result.
     * This setting is only taken into account if result is refined.
     *
     * @param keepCovariance true if covariance must be kept after refining result,
     *                       false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.keepCovariance = keepCovariance;
    }

    /**
     * Returns quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation always returns null.
     * Subclasses using quality scores must implement proper behavior.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return null;
    }

    /**
     * Sets quality scores corresponding to each measurement.
     * The larger the score value the better the quality of the sample.
     * This implementation makes no action.
     * Subclasses using quality scores must implement proper behaviour.
     *
     * @param qualityScores quality scores corresponding to each pair of
     *                      matched points.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3075
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3095
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2837
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4113
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3436
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4367
return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3704
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5039
return evaluateCommonAxis(i, params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];

        final var m12 = result[1];
        final var m22 = result[2];

        final var m13 = result[3];
        final var m23 = result[4];
        final var m33 = result[5];

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, 0.0);
        mm.setElementAtIndex(2, 0.0);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, 0.0);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        setResult(mm);

        // at this point covariance is expressed in terms of M, and must
        // be expressed in terms of Mg.
        // We know that:
        // Mg = M - I
        // Gg = M * G

        // M = [m11  m12  m13]
        //     [0    m22  m23]
        //     [0    0    m33]

        // G = [g11  g12  g13] = 0
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [0          m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [0          0           m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [0    m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [0    0    m33][g31  g32  g33]

        // Defining the linear application:
        // F(M) = F(m11, m12, m22, m32=0, m13, m23, m33)
        // as:
        // [sx] =   [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]
        // [gg11]   [0]
        // [gg21]   [0]
        // [gg31]   [0]
        // [gg12]   [0]
        // [gg22]   [0]
        // [gg32]   [0]
        // [gg13]   [0]
        // [gg23]   [0]
        // [gg33]   [0]

        // Then the Jacobian of F(M) is:
        // J = [1    0    0    0    0    0]
        //     [0    0    1    0    0    0]
        //     [0    0    0    0    0    1]
        //     [0    1    0    0    0    0]
        //     [0    0    0    1    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    1    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]
        //     [0    0    0    0    0    0]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS);

        jacobian.setElementAt(0, 0, 1.0);
        jacobian.setElementAt(1, 2, 1.0);
        jacobian.setElementAt(2, 5, 1.0);

        jacobian.setElementAt(3, 1, 1.0);
        jacobian.setElementAt(4, 3, 1.0);

        jacobian.setElementAt(6, 4, 1.0);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform general calibration when G-dependant cross
     * biases are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6260
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5984
return create(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener,
                DEFAULT_ROBUST_METHOD);
    }

    /**
     * Computes error of preliminary result respect a given measurement for current gravity norm.
     *
     * @param measurement       a measurement.
     * @param preliminaryResult a preliminary result.
     * @return computed error.
     */
    protected double computeError(
            final StandardDeviationBodyKinematics measurement, final PreliminaryResult preliminaryResult) {

        try {
            // We know that measured specific force is:
            // fmeas = ba + (I + Ma) * ftrue

            // fmeas - ba = (I + Ma) * ftrue

            // ftrue = (I + Ma)^-1 * (fmeas - ba)

            // We know that ||ftrue|| should be equal to the gravity value at current Earth
            // position
            // ||ftrue|| = g ~ 9.81 m/s^2

            final var estBiases = preliminaryResult.estimatedBiases;
            final var estMa = preliminaryResult.estimatedMa;

            if (identity == null) {
                identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp1 == null) {
                tmp1 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp2 == null) {
                tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp3 == null) {
                tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (tmp4 == null) {
                tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            identity.add(estMa, tmp1);

            Utils.inverse(tmp1, tmp2);

            final var kinematics = measurement.getKinematics();
            final var fmeasX = kinematics.getFx();
            final var fmeasY = kinematics.getFy();
            final var fmeasZ = kinematics.getFz();

            final var bx = estBiases[0];
            final var by = estBiases[1];
            final var bz = estBiases[2];

            tmp3.setElementAtIndex(0, fmeasX - bx);
            tmp3.setElementAtIndex(1, fmeasY - by);
            tmp3.setElementAtIndex(2, fmeasZ - bz);

            tmp2.multiply(tmp3, tmp4);

            final var norm = Utils.normF(tmp4);
            final var diff = groundTruthGravityNorm - norm;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3077
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3438
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2955
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4192
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4369
}

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3097
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2839
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4115
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2955
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4192
}

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4110
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4677
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5460
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6059
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS_AND_CROSS_BIASES);

        jacobian.setElementAt(0, 0, m11);
        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(0, 6, by);
        jacobian.setElementAt(0, 9, bz);

        jacobian.setElementAt(1, 0, m21);
        jacobian.setElementAt(1, 1, m22);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(1, 4, bx);
        jacobian.setElementAt(1, 7, by);
        jacobian.setElementAt(1, 10, bz);

        jacobian.setElementAt(2, 0, m31);
        jacobian.setElementAt(2, 1, m32);
        jacobian.setElementAt(2, 2, m33);
        jacobian.setElementAt(2, 5, bx);
        jacobian.setElementAt(2, 8, by);
        jacobian.setElementAt(2, 11, bz);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 7, 1.0);
        jacobian.setElementAt(5, 11, 1.0);

        jacobian.setElementAt(6, 6, 1.0);
        jacobian.setElementAt(7, 9, 1.0);
        jacobian.setElementAt(8, 4, 1.0);

        jacobian.setElementAt(9, 10, 1.0);
        jacobian.setElementAt(10, 5, 1.0);
        jacobian.setElementAt(11, 8, 1.9);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3189
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2199
}

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
     *
     * @return variance of estimated x coordinate of gyroscope bias or null if not available.
     */
    public Double getEstimatedBiasXVariance() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5222
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2606
estimatedMa.setElementAt(i, i, estimatedMa.getElementAt(i, i) - 1.0);
        }

        // since only a constant term is subtracted, covariance is preserved
        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final var m11 = params[0];
        final var m21 = params[1];
        final var m31 = params[2];

        final var m12 = params[3];
        final var m22 = params[4];
        final var m32 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        return evaluate(m11, m21, m31, m12, m22, m32, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 6.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final var m11 = params[0];

        final var m12 = params[1];
        final var m22 = params[2];

        final var m13 = params[3];
        final var m23 = params[4];
        final var m33 = params[5];

        return evaluate(m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33) throws EvaluationException {

        // fmeas = M*(ftrue + b)

        // ftrue = M^-1*fmeas - b

        try {
            if (fmeas == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 335
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 339
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 338
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 341
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5869
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6649
private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33) throws EvaluationException {

        // Ωmeas = M*(Ωtrue + b)
        // Ωtrue = M^-1 * Ωmeas - b

        try {
            if (measAngularRate == null) {
                measAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (m == null) {
                m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (invM == null) {
                invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (b == null) {
                b = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (trueAngularRate == null) {
                trueAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            measAngularRate.setElementAtIndex(0, measAngularRateX);
            measAngularRate.setElementAtIndex(1, measAngularRateY);
            measAngularRate.setElementAtIndex(2, measAngularRateZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, biasX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1507
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1511
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 409
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 421
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4670
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5211
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2758
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3188
running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1507
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 336
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 340
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 339
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1511
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 342
super(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2062
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2356
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3932
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3684
running = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5213
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3190
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2136
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2245
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2203
}


    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
     *
     * @return variance of estimated x coordinate of accelerometer bias or null if not available.
     */
    public Double getEstimatedBiasFxVariance() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1869
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1622
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2350
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2484
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2591
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2724
}

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bx, by, bz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets variance of estimated x coordinate of magnetometer bias expressed in
     * squared Teslas (T^2).
     *
     * @return variance of estimated x coordinate of magnetometer bias or null if
     * not available.
     */
    public Double getEstimatedHardIronXVariance() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4672
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2760
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2136
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2245
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2203
}

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }

    /**
     * Gets estimated chi square value.
     *
     * @return estimated chi square value.
     */
    @Override
    public double getEstimatedChiSq() {
        return estimatedChiSq;
    }

    /**
     * Gets estimated mean square error respect to provided measurements.
     *
     * @return estimated mean square error respect to provided measurements.
     */
    @Override
    public double getEstimatedMse() {
        return estimatedMse;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1507
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 342
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1941
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 7918
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6882
initialMm, listener, DEFAULT_ROBUST_METHOD);
    }

    /**
     * Computes error of a preliminary result respect a given measurement.
     *
     * @param measurement       a measurement.
     * @param preliminaryResult a preliminary result.
     * @return computed error.
     */
    protected double computeError(
            final StandardDeviationBodyMagneticFluxDensity measurement, final PreliminaryResult preliminaryResult) {

        try {
            // The magnetometer model is:
            // mBmeas = bm + (I + Mm) * mBtrue

            // mBmeas - bm = (I + Mm) * mBtrue

            // mBtrue = (I + Mm)^-1 * (mBmeas - ba)

            // We know that ||mBtrue||should be equal to the magnitude of the
            // Earth magnetic field at provided location

            final var estimatedBiases = preliminaryResult.estimatedHardIron;
            final var estMm = preliminaryResult.estimatedMm;

            if (identity == null) {
                identity = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }

            if (tmp1 == null) {
                tmp1 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }

            if (tmp2 == null) {
                tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp3 == null) {
                tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (tmp4 == null) {
                tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            identity.add(estMm, tmp1);

            Utils.inverse(tmp1, tmp2);

            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bx = estimatedBiases[0];
            final var by = estimatedBiases[1];
            final var bz = estimatedBiases[2];

            tmp3.setElementAtIndex(0, bMeasX - bx);
            tmp3.setElementAtIndex(1, bMeasY - by);
            tmp3.setElementAtIndex(2, bMeasZ - bz);

            tmp2.multiply(tmp3, tmp4);

            final var norm = Utils.normF(tmp4);
            final var diff = groundTruthMagneticFluxDensityNorm - norm;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1948
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1940
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1944
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1856
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1587
state = new INSLooselyCoupledKalmanState();
        }
    }

    /**
     * Corrects provided kinematics by taking into account currently estimated
     * specific force and angular rate biases.
     * This method stores the result into the variable member containing corrected
     * kinematics values.
     *
     * @param kinematics kinematics instance to be corrected.
     */
    private void correctKinematics(final BodyKinematics kinematics) {
        if (correctedKinematics == null) {
            correctedKinematics = new BodyKinematics();
        }

        final double accelBiasX;
        final double accelBiasY;
        final double accelBiasZ;
        final double gyroBiasX;
        final double gyroBiasY;
        final double gyroBiasZ;
        if (state != null) {
            accelBiasX = getValueOrZero(state.getAccelerationBiasX());
            accelBiasY = getValueOrZero(state.getAccelerationBiasY());
            accelBiasZ = getValueOrZero(state.getAccelerationBiasZ());
            gyroBiasX = getValueOrZero(state.getGyroBiasX());
            gyroBiasY = getValueOrZero(state.getGyroBiasY());
            gyroBiasZ = getValueOrZero(state.getGyroBiasZ());
        } else {
            accelBiasX = 0.0;
            accelBiasY = 0.0;
            accelBiasZ = 0.0;
            gyroBiasX = 0.0;
            gyroBiasY = 0.0;
            gyroBiasZ = 0.0;
        }

        final var fx = kinematics.getFx();
        final var fy = kinematics.getFy();
        final var fz = kinematics.getFz();
        final var angularRateX = kinematics.getAngularRateX();
        final var angularRateY = kinematics.getAngularRateY();
        final var angularRateZ = kinematics.getAngularRateZ();

        correctedKinematics.setSpecificForceCoordinates(fx - accelBiasX, fy - accelBiasY, fz - accelBiasZ);
        correctedKinematics.setAngularRateCoordinates(
                angularRateX - gyroBiasX,
                angularRateY - gyroBiasY,
                angularRateZ - gyroBiasZ);
    }

    /**
     * Returns provided value if not infinity and not NaN.
     *
     * @param value value to be returned.
     * @return value or 0.0.
     */
    private double getValueOrZero(final double value) {
        if (Double.isNaN(value) || Double.isInfinite(value)) {
            return 0.0;
        } else {
            return value;
        }
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1058
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1194
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
            a.setElementAt(i, 3, bTrueX);
            a.setElementAt(i, 6, bTrueY);
            a.setElementAt(i, 7, bTrueZ);

            b.setElementAtIndex(i, bMeasX - bTrueX);
            i++;

            a.setElementAt(i, 1, 1.0);
            a.setElementAt(i, 4, bTrueY);
            a.setElementAt(i, 8, bTrueZ);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3396
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3459
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1948
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1152
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3092
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2834
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4110
} catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 973
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 977
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2917
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2367
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5958
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5480
estimatedMg = preliminaryResult.estimatedMg;
            estimatedGg = preliminaryResult.estimatedGg;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed value and unit to radians per second.
     *
     * @param value angular speed value.
     * @param unit  unit of angular speed value.
     * @return converted value.
     */
    private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
        return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }

    /**
     * Fixes a measured kinematics instance using current
     *
     * @param measuredKinematics a measured kinematics instance.
     * @param result             instance where fixed values will be stored.
     * @throws AlgebraException if accelerometer or gyroscope parameters
     *                          contain numerical instabilities.
     */
    private void fixKinematics(final BodyKinematics measuredKinematics, final BodyKinematics result)
            throws AlgebraException {

        measuredSpecificForce[0] = measuredKinematics.getFx();
        measuredSpecificForce[1] = measuredKinematics.getFy();
        measuredSpecificForce[2] = measuredKinematics.getFz();
        accelerationFixer.fix(measuredSpecificForce, fixedSpecificForce);

        measuredAngularRate[0] = measuredKinematics.getAngularRateX();
        measuredAngularRate[1] = measuredKinematics.getAngularRateY();
        measuredAngularRate[2] = measuredKinematics.getAngularRateZ();
        angularRateFixer.fix(measuredAngularRate, fixedSpecificForce, fixedAngularRate);

        result.setSpecificForceCoordinates(fixedSpecificForce[0], fixedSpecificForce[1], fixedSpecificForce[2]);
        result.setAngularRateCoordinates(fixedAngularRate[0], fixedAngularRate[1], fixedAngularRate[2]);
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated gyroscope biases for each IMU axis expressed in radians per second
         * (rad/s).
         */
        private double[] estimatedBiases;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3095
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2837
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4113
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3187
running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3077
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3097
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2839
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4115
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3438
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2199
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4369
}

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3189
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2955
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4192
}

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
     * mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
     * gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
        return estimatedCovariance;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1507
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1571
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 336
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1593
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1511
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 340
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 339
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 342
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1941
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2770
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2739
public void setListener(final EasyGyroscopeCalibratorListener listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required sequences.
     *
     * @return minimum number of required sequences.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        if (commonAxisUsed) {
            if (estimateGDependentCrossBiases) {
                return MINIMUM_SEQUENCES_COMMON_Z_AXIS_AND_CROSS_BIASES;
            } else {
                return MINIMUM_SEQUENCES_COMMON_Z_AXIS;
            }
        } else {
            if (estimateGDependentCrossBiases) {
                return MINIMUM_SEQUENCES_GENERAL_AND_CROSS_BIASES;
            } else {
                return MINIMUM_SEQUENCES_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            reset();

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                if (estimateGDependentCrossBiases) {
                    calibrateCommonAxisAndGDependentCrossBiases();
                } else {
                    calibrateCommonAxis();
                }
            } else {
                if (estimateGDependentCrossBiases) {
                    calibrateGeneralAndGDependentCrossBiases();
                } else {
                    calibrateGeneral();
                }
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated gyroscope biases
     * expressed in radians per second (rad/s).
     *
     * @return array containing x,y,z components of estimated gyroscope biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4016
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4063
final KnownBiasTurntableGyroscopeCalibratorListener listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        if (commonAxisUsed) {
            if (estimateGDependentCrossBiases) {
                return MINIMUM_MEASUREMENTS_COMMON_Z_AXIS_AND_CROSS_BIASES;
            } else {
                return MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
            }
        } else {
            if (estimateGDependentCrossBiases) {
                return MINIMUM_MEASUREMENTS_GENERAL_AND_CROSS_BIASES;
            } else {
                return MINIMUM_MEASUREMENTS_GENERAL;
            }
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurementsOrSequences();
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                if (estimateGDependentCrossBiases) {
                    calibrateCommonAxisAndGDependentCrossBiases();
                } else {
                    calibrateCommonAxis();
                }
            } else {
                if (estimateGDependentCrossBiases) {
                    calibrateGeneralAndGDependentCrossBiases();
                } else {
                    calibrateGeneral();
                }
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException
                       | InvalidSourceAndDestinationFrameTypeException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1928
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3681
} catch (final AlgebraException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4667
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1148
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2755
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3095
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2837
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4113
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1899
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4448
running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated position.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2059
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1928
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5723
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5473
return create(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener,
                DEFAULT_ROBUST_METHOD);
    }

    /**
     * Computes error of preliminary result respect a given measurement for current gravity norm.
     *
     * @param measurement       a measurement.
     * @param preliminaryResult a preliminary result.
     * @return computed error.
     */
    protected double computeError(
            final StandardDeviationBodyKinematics measurement, final PreliminaryResult preliminaryResult) {

        try {
            // We know that measured specific force is:
            // fmeas = ba + (I + Ma) * ftrue

            // fmeas - ba = (I + Ma) * ftrue

            // ftrue = (I + Ma)^-1 * (fmeas - ba)

            // We know that ||ftrue|| should be equal to the gravity value at current Earth
            // position
            // ||ftrue|| = g ~ 9.81 m/s^2

            final var estMa = preliminaryResult.estimatedMa;

            if (identity == null) {
                identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp1 == null) {
                tmp1 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp2 == null) {
                tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp3 == null) {
                tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (tmp4 == null) {
                tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            identity.add(estMa, tmp1);

            Utils.inverse(tmp1, tmp2);

            final var kinematics = measurement.getKinematics();
            final var fmeasX = kinematics.getFx();
            final var fmeasY = kinematics.getFy();
            final var fmeasZ = kinematics.getFz();

            tmp3.setElementAtIndex(0, fmeasX - biasX);
            tmp3.setElementAtIndex(1, fmeasY - biasY);
            tmp3.setElementAtIndex(2, fmeasZ - biasZ);

            tmp2.multiply(tmp3, tmp4);

            final var norm = Utils.normF(tmp4);
            final var diff = groundTruthGravityNorm - norm;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2199
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2955
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4192
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1901
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4450
}

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration solution.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3075
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1155
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3436
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3187
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1899
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4448
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4367
return false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated gyroscope x-axis scale factor.
     *
     * @return estimated gyroscope x-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-axis scale factor.
     *
     * @return estimated gyroscope y-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated gyroscope z-axis scale factor.
     *
     * @return estimated gyroscope z-axis scale factor or null
     * if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated gyroscope x-y cross-coupling error.
     *
     * @return estimated gyroscope x-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated gyroscope x-z cross-coupling error.
     *
     * @return estimated gyroscope x-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated gyroscope y-x cross-coupling error.
     *
     * @return estimated gyroscope y-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated gyroscope y-z cross-coupling error.
     *
     * @return estimated gyroscope y-z cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated gyroscope z-x cross-coupling error.
     *
     * @return estimated gyroscope z-x cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated gyroscope z-y cross-coupling error.
     *
     * @return estimated gyroscope z-y cross-coupling error or null
     * if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     * This instance allows any 3x3 matrix.
     *
     * @return estimated G-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1436
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 982
final var g33 = unknowns.getElementAtIndex(14);

        fillMg(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
        fillGg(g11, g12, g13, g21, g22, g23, g31, g32, g33);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateGeneral() throws AlgebraException {
        // The gyroscope model is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        // [Ωmeasx] = [bx] + ( [1+sx  mxy    mxz ]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [myx   1+sy   myz ]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [mzx   mzy    1+sz]  [Ωtruez]   [g31   g32   g33][ftruez]

        // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
        // g31, g32, g33
        // Reordering:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy - Ωtruey - by = myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz - Ωtruez - bz = mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // [Ωtruex  0       0       Ωtruey  Ωtruez  0       0       0       0       ftruex  ftruey  ftruez  0       0       0       0       0       0     ][sx ] =  [Ωmeasx - Ωtruex - bx]
        // [0       Ωtruey  0       0       0       Ωtruex  Ωtruez  0       0       0       0       0       ftruex  ftruey  ftruez  0       0       0     ][sy ]    [Ωmeasy - Ωtruey - by]
        // [0       0       Ωtruez  0       0       0       0       Ωtruex  Ωtruey  0       0       0       0       0       0       ftruex  ftruey  ftruez][sz ]    [Ωmeasz - Ωtruez - bz]
        //                                                                                                                                                 [mxy]
        //                                                                                                                                                 [mxz]
        //                                                                                                                                                 [myx]
        //                                                                                                                                                 [myz]
        //                                                                                                                                                 [mzx]
        //                                                                                                                                                 [mzy]
        //                                                                                                                                                 [g11]
        //                                                                                                                                                 [g12]
        //                                                                                                                                                 [g13]
        //                                                                                                                                                 [g21]
        //                                                                                                                                                 [g22]
        //                                                                                                                                                 [g23]
        //                                                                                                                                                 [g31]
        //                                                                                                                                                 [g32]
        //                                                                                                                                                 [g33]

        final var expectedKinematics = new BodyKinematics();

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, GENERAL_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3095
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2837
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4113
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 690
running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1157
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 692
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2199
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2955
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4192
}

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
        return estimatedMg;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     *
     * @return a 3x3 matrix containing g-dependent cross biases.
     */
    @Override
    public Matrix getEstimatedGg() {
        return estimatedGg;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5748
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6518
private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33,
                            final double g11, final double g21, final double g31,
                            final double g12, final double g22, final double g32,
                            final double g13, final double g23, final double g33) throws EvaluationException {

        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
        // Ωmeas = M*(Ωtrue + b + G * ftrue)

        // M = I + Mg
        // bg = M*b --> b = M^-1*bg
        // Gg = M*G --> G = M^-1*Gg

        // Ωtrue = M^-1 * Ωmeas - b - G*ftrue

        try {
            if (measAngularRate == null) {
                measAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (fmeas == null) {
                fmeas = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (m == null) {
                m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (invM == null) {
                invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (b == null) {
                b = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (g == null) {
                g = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (trueAngularRate == null) {
                trueAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (ftrue == null) {
                ftrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (tmp == null) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2138
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2276
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, bTrueX);
            a.setElementAt(i, 1, 0.0);
            a.setElementAt(i, 2, 0.0);
            a.setElementAt(i, 3, bTrueY);
            a.setElementAt(i, 4, bTrueZ);
            a.setElementAt(i, 5, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6123
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3260
setResult(crossCoupling, bias);

        // at this point covariance is expressed in terms of b and M, and must
        // be expressed in terms of ba and Ma.
        // We know that:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [0    	m22 	m23]
        //     [0    	0    	m33]

        // m21 = m31 = m32 = 0

        // and that ba and Ma are expressed as:
        // Ma = M - I
        // ba = M * b

        // Ma = [m11 - 1    m12         m13    ] =  [sx     mxy     mxz]
        //      [0          m22 - 1     m23    ]    [0      sy      myz]
        //      [0          0           m33 - 1]    [0      0       sz ]

        // ba = [m11 * bx + m12 * by + m13 * bz] = 	[bax]
        //      [           m22 * by + m23 * bz]	[bay]
        //      [                      m33 * bz]	[baz]

        // Defining the linear application:
        // F(b, M) = F(bx, by, bz, m11, m12, m22, m13, m23, m33)
        // as:
        // [bax] = 	[m11 * bx + m12 * by + m13 * bz]
        // [bay]	[m22 * by + m23 * bz]
        // [baz]	[m33 * bz]
        // [sx]		[m11 - 1]
        // [sy]		[m22 - 1]
        // [sz]		[m33 -1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]

        // Then the Jacobian of F(b, M) is:
        // J = 	[m11  m12  m13  bx  by  0   bz  0   0 ]
        //	    [0    m22  m23  0   0   by  0   bz  0 ]
        //	    [0    0    m33  0   0   0   0   0   bz]
        //	    [0    0    0    1   0   0   0   0   0 ]
        //	    [0    0    0    0   0   1   0   0   0 ]
        //	    [0    0    0    0   0   0   0   0   1 ]
        //	    [0    0    0    0   1   0   0   0   0 ]
        //	    [0    0    0    0   0   0   1   0   0 ]
        //	    [0    0    0    0   0   0   0   0   0 ]
        //	    [0    0    0    0   0   0   0   1   0 ]
        //	    [0    0    0    0   0   0   0   0   0 ]
        //	    [0    0    0    0   0   0   0   0   0 ]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);

        jacobian.setElementAt(0, 0, m11);

        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(1, 1, m22);

        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(2, 2, m33);

        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(3, 3, 1.0);

        jacobian.setElementAt(0, 4, by);
        jacobian.setElementAt(6, 4, 1.0);

        jacobian.setElementAt(1, 5, by);
        jacobian.setElementAt(4, 5, 1.0);

        jacobian.setElementAt(0, 6, bz);
        jacobian.setElementAt(7, 6, 1.0);

        jacobian.setElementAt(1, 7, bz);
        jacobian.setElementAt(9, 7, 1.0);

        jacobian.setElementAt(2, 8, bz);
        jacobian.setElementAt(5, 8, 1.0);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal cross-coupling matrix.
     * @param b internal bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b) throws AlgebraException {
        // Because:
        // M = I + Ma
        // b = M^-1*ba

        // Then:
        // Ma = M - I
        // ba = M*b

        if (estimatedBiases == null) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4354
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5717
setResult(m, b);

        // at this point covariance is expressed in terms of b, M and G, and must
        // be expressed in terms of bg, Mg and Gg.
        // We know that:
        // bg = M * b
        // Mg = M - I
        // Gg = M * G = 0

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11  m12  m13]
        //     [0    m22  m23]
        //     [0    0    m33]

        // G = [g11  g12  g13] = 0
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // bg = [m11  m12  m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
        //      [0    m22  m23][by]   [           m22 * by + m23 * bz]   [bgy]
        //      [0    0    m33][bz]   [                      m33 * bz]   [bgz]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [0          m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [0          0           m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = 0
        //      [gg21  gg22  gg23]
        //      [gg31  gg32  gg33]

        // Defining the linear application:
        // F(b, M) = F(bx, by, bz, m11, m12, m22, m13, m23, m33)
        // as:
        // [bgx] =  [m11 * bx + m12 * by + m13 * bz]
        // [bgy]    [           m22 * by + m23 * bz]
        // [bgz]    [                      m33 * bz]
        // [sx]     [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]
        // [gg11]   [0]
        // [gg21]   [0]
        // [gg31]   [0]
        // [gg12]   [0]
        // [gg22]   [0]
        // [gg32]   [0]
        // [gg13]   [0]
        // [gg23]   [0]
        // [gg33]   [0]

        // Then the Jacobian of F(b, M) is:
        // J = [m11  m12  m13  bx   by   0    bz   0    0  ]
        //     [0    m22  m23  0    0    by   0    bz   0  ]
        //     [0    0    m33  0    0    0    0    0    bz ]
        //     [0    0    0    1    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    1  ]
        //     [0    0    0    0    1    0    0    0    0  ]
        //     [0    0    0    0    0    0    1    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    1    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0  ]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS);

        jacobian.setElementAt(0, 0, m11);
        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(0, 4, by);
        jacobian.setElementAt(0, 6, bz);

        jacobian.setElementAt(1, 1, m22);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(1, 5, by);
        jacobian.setElementAt(1, 7, bz);

        jacobian.setElementAt(2, 2, m33);
        jacobian.setElementAt(2, 8, bz);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 5, 1.0);
        jacobian.setElementAt(5, 8, 1.0);

        jacobian.setElementAt(6, 4, 1.0);
        jacobian.setElementAt(7, 6, 1.0);

        jacobian.setElementAt(9, 7, 1.0);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform general calibration when G-dependant cross
     * biases are ignored.
     *
     * @throws AlgebraException                         if accelerometer parameters prevent fixing
     *                                                  measured accelerometer values due to numerical instabilities.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4670
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2758
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1837
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2495
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2454
running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2062
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3684
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1867
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2589
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2722
running = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5228
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5665
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2612
estimatedMse = fitter.getMse();
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters for the general case.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the general purpose case.
     *               Must have length 9.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneral(final double[] params) throws EvaluationException {
        final var m11 = params[0];
        final var m21 = params[1];
        final var m31 = params[2];

        final var m12 = params[3];
        final var m22 = params[4];
        final var m32 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        return evaluate(m11, m21, m31, m12, m22, m32, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters when common z-axis is assumed.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param params array containing current parameters for the common z-axis case.
     *               Must have length 6.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateCommonAxis(final double[] params) throws EvaluationException {
        final var m11 = params[0];

        final var m12 = params[1];
        final var m22 = params[2];

        final var m13 = params[3];
        final var m23 = params[4];
        final var m33 = params[5];

        return evaluate(m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
    }

    /**
     * Computes estimated true specific force squared norm using current measured
     * specific force and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true specific force squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33) throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2136
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2245
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2203
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1839
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2497
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2456
}

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     * This is only available when result has been refined and covariance
     * is kept.
     *
     * @return estimated covariance matrix for estimated calibration parameters.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7467
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8596
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7946
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9056
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2064
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2358
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3934
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3686
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1622
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2350
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2484
}

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }

    /**
     * Gets estimated covariance matrix for estimated calibration parameters.
     * Diagonal elements of the matrix contains variance for the following
     * parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
     * myz, mzx, mzy.
     *
     * @return estimated covariance matrix for estimated position.
     */
    @Override
    public Matrix getEstimatedCovariance() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4670
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2758
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 679
running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5211
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1151
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3188
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1837
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2495
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2454
return false;
        }
    }


    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2062
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 848
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3684
running = false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2356
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3932
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1931
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1867
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2589
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2722
return false;
        }
    }

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1414
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1385
PROSACRobustEasyGyroscopeCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < EasyGyroscopeCalibrator.MINIMUM_SEQUENCES_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 6554
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 7060
}
    }

    /**
     * Converts a time instance expressed in milliseconds since epoch time
     * (January 1st, 1970 at midnight) to a decimal year.
     *
     * @param timestampMillis milliseconds value to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Long timestampMillis) {
        if (timestampMillis == null) {
            return null;
        }

        final var calendar = new GregorianCalendar();
        calendar.setTimeInMillis(timestampMillis);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained ina date object to a
     * decimal year.
     *
     * @param date a time instance to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Date date) {
        if (date == null) {
            return null;
        }

        final var calendar = new GregorianCalendar();
        calendar.setTime(date);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained in a gregorian calendar to a
     * decimal year.
     *
     * @param calendar calendar containing a specific instant to be
     *                 converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final GregorianCalendar calendar) {
        if (calendar == null) {
            return null;
        }

        return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
    }

    /**
     * Converts provided ECEF position to position expressed in NED
     * coordinates.
     *
     * @param position ECEF position to be converted.
     * @return converted position expressed in NED coordinates.
     */
    private static NEDPosition convertPosition(final ECEFPosition position) {
        final var velocity = new NEDVelocity();
        final var result = new NEDPosition();
        ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                position.getX(), position.getY(), position.getZ(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts magnetic flux density value and unit to Teslas.
     *
     * @param value magnetic flux density value.
     * @param unit  unit of magnetic flux density value.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
        return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Converts magnetic flux density instance to Teslas.
     *
     * @param magneticFluxDensity magnetic flux density instance to be converted.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
        return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 936
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1229
this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @return known accelerometer bias.
     */
    @Override
    public AccelerationTriad getBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known accelerometer bias.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBias(final AccelerationTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
        biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
        biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1153
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 681
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2136
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2245
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2203
}

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
        return estimatedMa;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 850
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1933
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1622
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2350
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2484
}

    /**
     * Gets estimated magnetometer soft-iron matrix containing scale factors
     * and cross coupling errors.
     * This is the product of matrix Tm containing cross coupling errors and Km
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Km = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tm = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
     * becomes upper diagonal:
     * <pre>
     *     Mm = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated magnetometer soft-iron scale factors and cross coupling errors,
     * or null if not available.
     */
    @Override
    public Matrix getEstimatedMm() {
        return estimatedMm;
    }

    /**
     * Gets estimated x-axis scale factor.
     *
     * @return estimated x-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSx() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
    }

    /**
     * Gets estimated y-axis scale factor.
     *
     * @return estimated y-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSy() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
    }

    /**
     * Gets estimated z-axis scale factor.
     *
     * @return estimated z-axis scale factor or null if not available.
     */
    @Override
    public Double getEstimatedSz() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
    }

    /**
     * Gets estimated x-y cross-coupling error.
     *
     * @return estimated x-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxy() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
    }

    /**
     * Gets estimated x-z cross-coupling error.
     *
     * @return estimated x-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMxz() {
        return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
    }

    /**
     * Gets estimated y-x cross-coupling error.
     *
     * @return estimated y-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyx() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
    }

    /**
     * Gets estimated y-z cross-coupling error.
     *
     * @return estimated y-z cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMyz() {
        return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
    }

    /**
     * Gets estimated z-x cross-coupling error.
     *
     * @return estimated z-x cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzx() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
    }

    /**
     * Gets estimated z-y cross-coupling error.
     *
     * @return estimated z-y cross-coupling error or null if not available.
     */
    @Override
    public Double getEstimatedMzy() {
        return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 538
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 538
PROSACRobustKnownFrameMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2143
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2142
PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3621
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3585
public void setListener(final KnownFrameMagnetometerNonLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return magneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.magneticModel = magneticModel;
    }

    /**
     * Estimates magnetometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 163
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 169
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 166
final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1869
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1773
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1795
PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum required
     *                                  number of samples (10 or 13).
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 575
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 560
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return sequences.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(sequences.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 879
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 884
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2007
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2049
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1137
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1179
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC,
                SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4879
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5694
}

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws WrongSizeException never happens.
     */
    private void setInputData() throws WrongSizeException {

        final var g = groundTruthGravityNorm;
        final var g2 = g * g;

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
        final var y = new double[numMeasurements];
        final var specificForceStandardDeviations = new double[numMeasurements];
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();

            final var fx = measuredKinematics.getFx();
            final var fy = measuredKinematics.getFy();
            final var fz = measuredKinematics.getFz();

            x.setElementAt(i, 0, fx);
            x.setElementAt(i, 1, fy);
            x.setElementAt(i, 2, fz);

            y[i] = g2;

            specificForceStandardDeviations[i] = measurement.getSpecificForceStandardDeviation();

            i++;
        }

        fitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // accelerometer model can be better expressed as:
        // fmeas = T*K*(ftrue + b)
        // fmeas = M*(ftrue + b)
        // fmeas = M*ftrue + M*b

        // where:
        // M = I + Ma
        // ba = M*b = (I + Ma)*b --> b = M^-1*ba

        // We know that the norm of the true specific force is equal to the amount
        // of gravity at a certain Earth position
        // ||ftrue|| = ||g|| ~ 9.81 m/s^2

        // Hence:
        // fmeas - M*b = M*ftrue

        // M^-1 * (fmeas - M*b) = ftrue

        // ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
        // ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
        // ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
        // ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [m21 	m22 	m23]
        //     [m31 	m32 	m33]

        // Notice that bias b is known, hence only terms in matrix M need to be estimated

        final var gradientEstimator = new GradientEstimator(this::evaluateGeneral);

        final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMa());
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 740
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 746
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        gravityNorm = computeGravityNorm();

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1704
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 535
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1710
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 539
PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 943
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2883
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1251
this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @return array containing coordinate of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known gyroscope bias as an array.
     * Array values are expressed in radians per second (rad/s).
     *
     * @param bias known gyroscope bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2141
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2138
PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.this,
                            progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5292
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6329
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5813
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6830
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2266
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2837
private void setInputData() throws WrongSizeException {
        final var gtb = groundTruthMagneticFluxDensityNorm;
        final var gtb2 = gtb * gtb;

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, BodyMagneticFluxDensity.COMPONENTS);
        final var y = new double[numMeasurements];
        final var specificForceStandardDeviations = new double[numMeasurements];
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            final var bmeasuredX = measuredMagneticFluxDensity.getBx();
            final var bmeasuredY = measuredMagneticFluxDensity.getBy();
            final var bmeasuredZ = measuredMagneticFluxDensity.getBz();

            x.setElementAt(i, 0, bmeasuredX);
            x.setElementAt(i, 1, bmeasuredY);
            x.setElementAt(i, 2, bmeasuredZ);

            y[i] = gtb2;

            specificForceStandardDeviations[i] = measurement.getMagneticFluxDensityStandardDeviation();

            i++;
        }

        fitter.setInputData(x, y, specificForceStandardDeviations);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws FittingException                         if Levenberg-Marquardt fails for numerical reasons.
     * @throws AlgebraException                         if there are numerical instabilities that prevent
     *                                                  matrix inversion.
     * @throws com.irurueta.numerical.NotReadyException never happens.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The magnetometer model is:
        // bmeas = bm + (I + Mm) * btrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // bmeas = bm + (I + Mm) * btrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // magnetometer model can be better expressed as:
        // bmeas = T*K*(btrue + b)
        // bmeas = M*(btrue + b)
        // bmeas = M*btrue + M*b

        // where:
        // M = I + Mm
        // bm = M*b = (I + Mm)*b --> b = M^-1*bm

        // We know that the norm of the true body magnetic flux density
        // is equal to the amount of Earth magnetic flux density at provided
        // position and timestamp
        // ||btrue|| = ||bEarth|| --> from 30 µT to 60 µT

        // Hence:
        // bmeas - M*b = M*btrue

        // M^-1 * (bmeas - M*b) = btrue

        // ||bEarth||^2 = ||btrue||^2 = (M^-1 * (bmeas - M*b))^T * (M^-1 * (bmeas - M*b))
        // ||bEarth||^2 = (bmeas - M*b)^T*(M^-1)^T * M^-1 * (bmeas - M*b)
        // ||bEarth||^2 = (bmeas - M * b)^T * ||M^-1||^2 * (bmeas - M * b)
        // ||bEarth||^2 = ||bmeas - M * b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [m21 	m22 	m23]
        //     [m31 	m32 	m33]

        final var gradientEstimator = new GradientEstimator(this::evaluateGeneral);

        final var initialM = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
        initialM.add(getInitialMm());
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7631
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8760
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8107
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9217
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2740
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1534
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
        final var result = new BodyKinematics();
        estimateKinematics(timeInterval, frame, oldFrame, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z) {
        final var result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z) {
        final var result = new BodyKinematics();
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
        return result;
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static BodyKinematics estimateKinematicsAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1621
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1653
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1397
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1367
PROMedSRobustEasyGyroscopeCalibrator.this, progress);
                        }
                    }
                });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            inliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null
                || qualityScores.length < TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10594
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 11100
estimatedMg = preliminaryResult.estimatedMg;
            estimatedGg = preliminaryResult.estimatedGg;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final var velocity = new ECEFVelocity();
        final var result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param value angular speed value.
     * @param unit  unit of angular speed value.
     * @return converted value.
     */
    private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
        return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }

    /**
     * Converts time instance to seconds.
     *
     * @param time time instance to be converted.
     * @return converted value.
     */
    private static double convertTime(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated gyroscope scale factors and cross coupling errors.
         * This is the product of matrix Tg containing cross coupling errors and Kg
         * containing scaling factors.
         * So that:
         * <pre>
         *     Mg = [sx    mxy  mxz] = Tg*Kg
         *          [myx   sy   myz]
         *          [mzx   mzy  sz ]
         * </pre>
         * Where:
         * <pre>
         *     Kg = [sx 0   0 ]
         *          [0  sy  0 ]
         *          [0  0   sz]
         * </pre>
         * and
         * <pre>
         *     Tg = [1          -alphaXy    alphaXz ]
         *          [alphaYx    1           -alphaYz]
         *          [-alphaZx   alphaZy     1       ]
         * </pre>
         * Hence:
         * <pre>
         *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
         *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
         *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
         * </pre>
         * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
         * are considered to be zero if the gyroscope z-axis is assumed to be the same
         * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
         * becomes upper diagonal:
         * <pre>
         *     Mg = [sx    mxy  mxz]
         *          [0     sy   myz]
         *          [0     0    sz ]
         * </pre>
         * Values of this matrix are unit-less.
         */
        private Matrix estimatedMg;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 794
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 801
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 164
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 687
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 170
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 167
super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5085
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4350
final int i, final double bx, final double by, final double bz,
            final double m11, final double m21, final double m31,
            final double m12, final double m22, final double m32,
            final double m13, final double m23, final double m33,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33) throws EvaluationException {

        try {
            final var measuredSequence = sequences.get(i);
            final var fixedSequence = fixedSequences.get(i);

            // generate new sequence using current parameters to fix angular rate measurements
            if (measuredSpecificForce == null) {
                measuredSpecificForce = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (trueSpecificForce == null) {
                trueSpecificForce = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (measuredAngularRate == null) {
                measuredAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (trueAngularRate == null) {
                trueAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (m == null) {
                m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (invM == null) {
                invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (b == null) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 882
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 879
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 884
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5445
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6482
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7386
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8514
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5963
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6980
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7863
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8973
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 521
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 519
PROMedSRobustKnownFrameMagnetometerCalibrator.this, progress);
                        }
                    }
                });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            setupWmmEstimator();

            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2129
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2125
PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            initialize();

            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 794
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 164
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 801
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 687
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 170
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 167
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 882
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 879
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 884
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 89
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 116
crossCouplingErrors = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } catch (final WrongSizeException ignore) {
            // never happens
        }
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @return bias values expressed in meters per squared second.
     */
    public Matrix getBias() {
        return new Matrix(bias);
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result will be stored.
     */
    public void getBias(final Matrix result) {
        bias.copyTo(result);
    }

    /**
     * Sets bias values expressed in meters per squared second (m/s^2).
     *
     * @param bias bias values expressed in meters per squared second.
     *             Must be 3x1.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    public void setBias(final Matrix bias) {
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        this.bias = bias;
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @return bias values expressed in meters per squared second.
     */
    public double[] getBiasArray() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBiasArray(result);
        return result;
    }

    /**
     * Gets bias values expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void getBiasArray(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        try {
            bias.toArray(result);
        } catch (final WrongSizeException ignore) {
            // never happens
        }
    }

    /**
     * Sets bias values expressed in meters per squared second (m/s^2).
     *
     * @param bias bias values expressed in meters per squared second (m/s^2).
     *             Must have length 3.
     * @throws IllegalArgumentException if provided array does not have
     *                                  length 3.
     */
    public void setBias(final double[] bias) {
        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }

        try {
            this.bias.fromArray(bias);
        } catch (final WrongSizeException ignore) {
            // never happens
        }
    }

    /**
     * Gets acceleration bias.
     *
     * @return acceleration bias.
     */
    public AccelerationTriad getBiasAsTriad() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1852
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1755
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1872
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1774
PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum required
     *                                  number of samples (10 or 13).
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < getMinimumRequiredMeasurements()) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1571
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1593
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3576
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3641
PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.this, progress);
                        }
                    }
                });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null
                || qualityScores.length < TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1941
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1948
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 188
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 190
final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 601
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 584
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return sequences.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(sequences.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7161
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7308
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7551
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8680
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8289
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8436
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7640
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7784
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8026
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9136
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias,
                    initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.*
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8894
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 908
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 763
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 770
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        gravityNorm = computeGravityNorm();

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1683
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 517
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1694
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 521
PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5128
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6577
m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
            b.setElementAtIndex(1, by);
            b.setElementAtIndex(2, bz);

            g.setElementAt(0, 0, g11);
            g.setElementAt(1, 0, g21);
            g.setElementAt(2, 0, g31);

            g.setElementAt(0, 1, g12);
            g.setElementAt(1, 1, g22);
            g.setElementAt(2, 1, g32);

            g.setElementAt(0, 2, g13);
            g.setElementAt(1, 2, g23);
            g.setElementAt(2, 2, g33);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3537
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4197
if (estimatedMg == null) {
            estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedMg.initialize(0.0);
        }

        estimatedMg.setElementAt(0, 0, sx);

        estimatedMg.setElementAt(0, 1, mxy);
        estimatedMg.setElementAt(1, 1, sy);

        estimatedMg.setElementAt(0, 2, mxz);
        estimatedMg.setElementAt(1, 2, myz);
        estimatedMg.setElementAt(2, 2, sz);

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedGg.setElementAtIndex(0, g11);
        estimatedGg.setElementAtIndex(1, g21);
        estimatedGg.setElementAtIndex(2, g31);
        estimatedGg.setElementAtIndex(3, g12);
        estimatedGg.setElementAtIndex(4, g22);
        estimatedGg.setElementAtIndex(5, g32);
        estimatedGg.setElementAtIndex(6, g13);
        estimatedGg.setElementAtIndex(7, g23);
        estimatedGg.setElementAtIndex(8, g33);

        estimatedCovariance = fitter.getCovar();

        // propagate covariance matrix so that all parameters are taken into
        // account in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
        // g11, g21, g31, g12, g22, g32, g13, g23, g33

        // We define a lineal function mapping original parameters for the common
        // axis case to the general case
        // [sx'] =   [1  0  0  0  0  0  0  0  0  0  0  0  0  0  0][sx]
        // [sy']     [0  1  0  0  0  0  0  0  0  0  0  0  0  0  0][sy]
        // [sz']     [0  0  1  0  0  0  0  0  0  0  0  0  0  0  0][sz]
        // [mxy']    [0  0  0  1  0  0  0  0  0  0  0  0  0  0  0][mxy]
        // [mxz']    [0  0  0  0  1  0  0  0  0  0  0  0  0  0  0][mxz]
        // [myx']    [0  0  0  0  0  0  0  0  0  0  0  0  0  0  0][myz]
        // [myz']    [0  0  0  0  0  1  0  0  0  0  0  0  0  0  0][g11]
        // [mzx']    [0  0  0  0  0  0  0  0  0  0  0  0  0  0  0][g21]
        // [mzy']    [0  0  0  0  0  0  0  0  0  0  0  0  0  0  0][g31]
        // [g11']    [0  0  0  0  0  0  1  0  0  0  0  0  0  0  0][g12]
        // [g21']    [0  0  0  0  0  0  0  1  0  0  0  0  0  0  0][g22]
        // [g31']    [0  0  0  0  0  0  0  0  1  0  0  0  0  0  0][g32]
        // [g12']    [0  0  0  0  0  0  0  0  0  1  0  0  0  0  0][g13]
        // [g22']    [0  0  0  0  0  0  0  0  0  0  1  0  0  0  0][g23]
        // [g32']    [0  0  0  0  0  0  0  0  0  0  0  1  0  0  0][g33]
        // [g13']    [0  0  0  0  0  0  0  0  0  0  0  0  1  0  0]
        // [g23']    [0  0  0  0  0  0  0  0  0  0  0  0  0  1  0]
        // [g33']    [0  0  0  0  0  0  0  0  0  0  0  0  0  0  1]

        // As defined in com.irurueta.statistics.MultivariateNormalDist,
        // if we consider the jacobian of the lineal application the matrix shown
        // above, then covariance can be propagated as follows
        final var jacobian = new Matrix(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
        for (var i = 0; i < 5; i++) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2123
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2120
PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.this,
                                    progress);
                        }
                    }
                });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
            throw new IllegalArgumentException();
        }

        this.qualityScores = qualityScores;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 814
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 185
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 823
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 597
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 708
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 581
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 190
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 187
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1593
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1571
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1690
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1645
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1678
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1941
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1940
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1944
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 876
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 984
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, 1.0);
            a.setElementAt(i, 3, fTrueX);
            a.setElementAt(i, 6, fTrueY);
            a.setElementAt(i, 7, fTrueZ);

            b.setElementAtIndex(i, fMeasX - fTrueX);
            i++;

            a.setElementAt(i, 1, 1.0);
            a.setElementAt(i, 4, fTrueY);
            a.setElementAt(i, 8, fTrueZ);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 708
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 818
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 826
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 712
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 194
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 191
super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5216
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6253
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7634
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8599
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5735
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6752
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8110
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9059
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 906
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 908
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1421
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 779
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates)..
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF-frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
     *                     along ECEF-frame axes.
     * @param vy           y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
     *                     along ECEF-frame axes.
     * @param vz           z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
     *                     along ECEF-frame axes.
     * @param oldVx        x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
     *                     resolved along ECEF-frame axes.
     * @param oldVy        y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
     *                     resolved along ECEF-frame axes.
     * @param oldVz        z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
     *                     resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m) and resolved along
     *                     ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m) and resolved along
     *                     ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m) and resolved along
     *                     ECEF-frame axes.
     * @param result       instance where estimated body kinematics will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        if (timeInterval < 0.0 || !ECEFFrame.isValidCoordinateTransformation(c)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/AccelerometerNonLinearCalibrator.java 24
com/irurueta/navigation/inertial/calibration/gyroscope/GyroscopeNonLinearCalibrator.java 24
com/irurueta/navigation/inertial/calibration/magnetometer/MagnetometerNonLinearCalibrator.java 24
public interface AccelerometerNonLinearCalibrator extends AccelerometerCalibrator {

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    double getInitialSx();

    /**
     * Sets initial x scaling factor.
     *
     * @param initialSx initial x scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialSx(final double initialSx) throws LockedException;

    /**
     * Gets initial y scaling factor.
     *
     * @return initial y scaling factor.
     */
    double getInitialSy();

    /**
     * Sets initial y scaling factor.
     *
     * @param initialSy initial y scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialSy(final double initialSy) throws LockedException;

    /**
     * Gets initial z scaling factor.
     *
     * @return initial z scaling factor.
     */
    double getInitialSz();

    /**
     * Sets initial z scaling factor.
     *
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialSz(final double initialSz) throws LockedException;

    /**
     * Gets initial x-y cross coupling error.
     *
     * @return initial x-y cross coupling error.
     */
    double getInitialMxy();

    /**
     * Sets initial x-y cross coupling error.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMxy(final double initialMxy) throws LockedException;

    /**
     * Gets initial x-z cross coupling error.
     *
     * @return initial x-z cross coupling error.
     */
    double getInitialMxz();

    /**
     * Sets initial x-z cross coupling error.
     *
     * @param initialMxz initial x-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMxz(final double initialMxz) throws LockedException;

    /**
     * Gets initial y-x cross coupling error.
     *
     * @return initial y-x cross coupling error.
     */
    double getInitialMyx();

    /**
     * Sets initial y-x cross coupling error.
     *
     * @param initialMyx initial y-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMyx(final double initialMyx) throws LockedException;

    /**
     * Gets initial y-z cross coupling error.
     *
     * @return initial y-z cross coupling error.
     */
    double getInitialMyz();

    /**
     * Sets initial y-z cross coupling error.
     *
     * @param initialMyz initial y-z cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMyz(final double initialMyz) throws LockedException;

    /**
     * Gets initial z-x cross coupling error.
     *
     * @return initial z-x cross coupling error.
     */
    double getInitialMzx();

    /**
     * Sets initial z-x cross coupling error.
     *
     * @param initialMzx initial z-x cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMzx(final double initialMzx) throws LockedException;

    /**
     * Gets initial z-y cross coupling error.
     *
     * @return initial z-y cross coupling error.
     */
    double getInitialMzy();

    /**
     * Sets initial z-y cross coupling error.
     *
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialMzy(final double initialMzy) throws LockedException;

    /**
     * Sets initial scaling factors.
     *
     * @param initialSx initial x scaling factor.
     * @param initialSy initial y scaling factor.
     * @param initialSz initial z scaling factor.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
            throws LockedException;

    /**
     * Sets initial cross coupling errors.
     *
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialCrossCouplingErrors(
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException;

    /**
     * Sets initial scaling factors and cross coupling errors.
     *
     * @param initialSx  initial x scaling factor.
     * @param initialSy  initial y scaling factor.
     * @param initialSz  initial z scaling factor.
     * @param initialMxy initial x-y cross coupling error.
     * @param initialMxz initial x-z cross coupling error.
     * @param initialMyx initial y-x cross coupling error.
     * @param initialMyz initial y-z cross coupling error.
     * @param initialMzx initial z-x cross coupling error.
     * @param initialMzy initial z-y cross coupling error.
     * @throws LockedException if calibrator is currently running.
     */
    void setInitialScalingFactorsAndCrossCouplingErrors(
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException;

    /**
     * Gets initial scale factors and cross coupling errors matrix.
     *
     * @return initial scale factors and cross coupling errors matrix.
     */
    Matrix getInitialMa();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1507
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1571
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 336
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1593
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1511
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3396
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 340
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3459
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 339
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 342
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1941
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1948
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1940
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1944
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1704
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1869
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1773
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 535
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1795
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1710
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3595
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 539
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3658
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2141
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2138
PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 708
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 910
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 818
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 826
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 712
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 194
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 191
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 906
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 908
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 595
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 321
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz, final Point3D position,
            final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz, final Point3D position,
            final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6360
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6083
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
            innerCalibrator.setMeasurements(meas);
            innerCalibrator.calibrate();

            innerCalibrator.getEstimatedBiases(result.estimatedBiases);
            result.estimatedMa = innerCalibrator.getEstimatedMa();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setInitialBias(preliminaryResult.estimatedBiases);
                innerCalibrator.setInitialMa(preliminaryResult.estimatedMa);
                innerCalibrator.setCommonAxisUsed(commonAxisUsed);
                innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3830
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3790
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3842
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3877
public void setMeasurements(final Collection<StandardDeviationBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final var result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {
        if (position != null) {
            final var velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    position.getX(), position.getY(), position.getZ(), 0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = convertPosition(position);
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_BODY_KINEMATICS_MEASUREMENT;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return false;
File Line
com/irurueta/navigation/inertial/calibration/StandardDeviationFrameBodyKinematics.java 1261
com/irurueta/navigation/inertial/calibration/StandardDeviationTimedBodyKinematics.java 345
public StandardDeviationFrameBodyKinematics(final StandardDeviationFrameBodyKinematics input) {
        copyFrom(input);
    }

    /**
     * Gets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @return standard deviation of measured specific force.
     */
    public double getSpecificForceStandardDeviation() {
        return specificForceStandardDeviation;
    }

    /**
     * Sets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(final double specificForceStandardDeviation) {
        if (specificForceStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        this.specificForceStandardDeviation = specificForceStandardDeviation;
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @return standard deviation of measured specific force.
     */
    public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
        return new Acceleration(specificForceStandardDeviation, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @param result instance where standard deviation of measured specific force will be
     *               stored.
     */
    public void getSpecificForceStandardDeviationAsAcceleration(final Acceleration result) {
        result.setValue(specificForceStandardDeviation);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets standard deviation of measured specific force.
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(final Acceleration specificForceStandardDeviation) {
        setSpecificForceStandardDeviation(convertAcceleration(specificForceStandardDeviation));
    }

    /**
     * Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @return standard deviation of measured angular rate.
     */
    public double getAngularRateStandardDeviation() {
        return angularRateStandardDeviation;
    }

    /**
     * Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
        if (angularRateStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        this.angularRateStandardDeviation = angularRateStandardDeviation;
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @return standard deviation of measured angular rate.
     */
    public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
        return new AngularSpeed(angularRateStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @param result instance where standard deviation of measured angular rate will be
     *               stored.
     */
    public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        result.setValue(angularRateStandardDeviation);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets standard deviation of measured angular rate.
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(final AngularSpeed angularRateStandardDeviation) {
        setAngularRateStandardDeviation(convertAngularSpeed(angularRateStandardDeviation));
    }

    /**
     * Copies data of provided instance into this instance.
     *
     * @param input instance to copy data from.
     */
    public void copyFrom(final StandardDeviationFrameBodyKinematics input) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5006
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5143
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5370
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6407
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6043
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6180
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6879
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8007
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5527
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5661
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5887
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6904
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6544
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6678
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7366
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8474
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 54
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 46
private static final double SCALING_THRESHOLD = 2e-5;

    /**
     * Alpha threshold.
     */
    private static final double ALPHA_THRESHOLD = 1e-8;

    /**
     * Number of rows.
     */
    private static final int ROWS = 3;

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7306
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8434
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7389
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7554
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8517
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8683
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7782
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8892
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7866
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8029
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8976
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9139
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 907
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1048
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, 1.0);
            a.setElementAt(i, 3, omegaTrueX);
            a.setElementAt(i, 6, omegaTrueY);
            a.setElementAt(i, 7, omegaTrueZ);
            a.setElementAt(i, 9, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/generators/AccelerometerAndGyroscopeMeasurementsGenerator.java 564
com/irurueta/navigation/inertial/calibration/generators/AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator.java 650
gyroscopeMeasurementsGenerator.setBaseNoiseLevelAbsoluteThreshold(baseNoiseLevelAbsoluteThreshold);
    }

    /**
     * Gets internal status of this generator.
     *
     * @return internal status of this generator.
     */
    public TriadStaticIntervalDetector.Status getStatus() {
        return accelerometerMeasurementsGenerator.getStatus();
    }

    /**
     * Gets accelerometer base noise level that has been detected during
     * initialization expressed in meters per squared second (m/s^2).
     * This is equal to the standard deviation of the accelerometer measurements
     * during initialization phase.
     *
     * @return accelerometer base noise level.
     */
    public double getAccelerometerBaseNoiseLevel() {
        return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevel();
    }

    /**
     * Gets accelerometer base noise level that has been detected during
     * initialization.
     * This is equal to the standard deviation of the accelerometer measurements
     * during initialization phase.
     *
     * @return measurement base noise level.
     */
    public Acceleration getAccelerometerBaseNoiseLevelAsMeasurement() {
        return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelAsMeasurement();
    }

    /**
     * Gets accelerometer base noise level that has been detected during
     * initialization.
     *
     * @param result instance where result will be stored.
     */
    public void getAccelerometerBaseNoiseLevelAsMeasurement(final Acceleration result) {
        accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelAsMeasurement(result);
    }

    /**
     * Gets accelerometer base noise level PSD (Power Spectral Density)
     * expressed in (m^2 * s^-3).
     *
     * @return accelerometer base noise level PSD.
     */
    public double getAccelerometerBaseNoiseLevelPsd() {
        return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelPsd();
    }

    /**
     * Gets accelerometer base noise level root PSD (Power Spectral Density)
     * expressed in (m * s^-1.5).
     *
     * @return accelerometer base noise level root PSD.
     */
    @Override
    public double getAccelerometerBaseNoiseLevelRootPsd() {
        return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelRootPsd();
    }

    /**
     * Gets threshold to determine static/dynamic period changes expressed in
     * meters per squared second (m/s^2).
     *
     * @return threshold to determine static/dynamic period changes.
     */
    public double getThreshold() {
        return accelerometerMeasurementsGenerator.getThreshold();
    }

    /**
     * Gets threshold to determine static/dynamic period changes.
     *
     * @return threshold to determine static/dynamic period changes.
     */
    public Acceleration getThresholdAsMeasurement() {
        return accelerometerMeasurementsGenerator.getThresholdAsMeasurement();
    }

    /**
     * Gets threshold to determine static/dynamic period changes.
     *
     * @param result instance where result will be stored.
     */
    public void getThresholdAsMeasurement(final Acceleration result) {
        accelerometerMeasurementsGenerator.getThresholdAsMeasurement(result);
    }

    /**
     * Gets number of samples that have been processed in a static period so far.
     *
     * @return number of samples that have been processed in a static period so far.
     */
    public int getProcessedStaticSamples() {
        return accelerometerMeasurementsGenerator.getProcessedStaticSamples();
    }

    /**
     * Gets number of samples that have been processed in a dynamic period so far.
     *
     * @return number of samples that have been processed in a dynamic period so far.
     */
    public int getProcessedDynamicSamples() {
        return accelerometerMeasurementsGenerator.getProcessedDynamicSamples();
    }

    /**
     * Indicates whether last static interval must be skipped.
     *
     * @return true if last static interval must be skipped.
     */
    public boolean isStaticIntervalSkipped() {
        return accelerometerMeasurementsGenerator.isStaticIntervalSkipped();
    }

    /**
     * Indicates whether last dynamic interval must be skipped.
     *
     * @return true if last dynamic interval must be skipped.
     */
    public boolean isDynamicIntervalSkipped() {
        return accelerometerMeasurementsGenerator.isDynamicIntervalSkipped();
    }

    /**
     * Indicates whether generator is running or not.
     *
     * @return true if generator is running, false otherwise.
     */
    public boolean isRunning() {
        return running;
    }

    /**
     * Processes a sample of data.
     *
     * @param sample sample of data to be processed.
     * @return true if provided samples has been processed, false if provided triad has been skipped because
     * generator previously failed. If generator previously failed, it will need to be reset before
     * processing additional samples.
     * @throws LockedException if generator is busy processing a previous sample.
     */
    public boolean process(final TimedBodyKinematics sample) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7015
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8143
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7159
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8287
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7497
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8605
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7638
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8746
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 420
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 206
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldFrame, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public void estimate(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5817
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5567
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
            innerCalibrator.setMeasurements(meas);
            innerCalibrator.calibrate();

            result.estimatedMa = innerCalibrator.getEstimatedMa();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ);
                innerCalibrator.setInitialMa(preliminaryResult.estimatedMa);
                innerCalibrator.setCommonAxisUsed(commonAxisUsed);
                innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 906
public void setThreshold(double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5448
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6332
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6833
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/StandardDeviationBodyKinematics.java 173
com/irurueta/navigation/inertial/calibration/StandardDeviationFrameBodyKinematics.java 1263
com/irurueta/navigation/inertial/calibration/StandardDeviationTimedBodyKinematics.java 347
}

    /**
     * Gets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @return standard deviation of measured specific force.
     */
    public double getSpecificForceStandardDeviation() {
        return specificForceStandardDeviation;
    }

    /**
     * Sets standard deviation of measured specific force expressed in meters per squared
     * second (m/s^2).
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(final double specificForceStandardDeviation) {
        if (specificForceStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        this.specificForceStandardDeviation = specificForceStandardDeviation;
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @return standard deviation of measured specific force.
     */
    public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
        return new Acceleration(specificForceStandardDeviation, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Gets standard deviation of measured specific force.
     *
     * @param result instance where standard deviation of measured specific force will be
     *               stored.
     */
    public void getSpecificForceStandardDeviationAsAcceleration(final Acceleration result) {
        result.setValue(specificForceStandardDeviation);
        result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets standard deviation of measured specific force.
     *
     * @param specificForceStandardDeviation standard deviation of measured specific force.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setSpecificForceStandardDeviation(final Acceleration specificForceStandardDeviation) {
        setSpecificForceStandardDeviation(convertAcceleration(specificForceStandardDeviation));
    }

    /**
     * Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @return standard deviation of measured angular rate.
     */
    public double getAngularRateStandardDeviation() {
        return angularRateStandardDeviation;
    }

    /**
     * Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
        if (angularRateStandardDeviation < 0.0) {
            throw new IllegalArgumentException();
        }

        this.angularRateStandardDeviation = angularRateStandardDeviation;
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @return standard deviation of measured angular rate.
     */
    public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
        return new AngularSpeed(angularRateStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Gets standard deviation of measured angular rate.
     *
     * @param result instance where standard deviation of measured angular rate will be
     *               stored.
     */
    public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
        result.setValue(angularRateStandardDeviation);
        result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets standard deviation of measured angular rate.
     *
     * @param angularRateStandardDeviation standard deviation of measured angular rate.
     * @throws IllegalArgumentException if provided value is negative.
     */
    public void setAngularRateStandardDeviation(final AngularSpeed angularRateStandardDeviation) {
        setAngularRateStandardDeviation(convertAngularSpeed(angularRateStandardDeviation));
    }

    /**
     * Copies data of provided instance into this instance.
     *
     * @param input instance to copy data from.
     */
    public void copyFrom(final StandardDeviationBodyKinematics input) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1683
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1852
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1755
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 517
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1872
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1774
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1694
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3576
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 521
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3641
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2123
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2120
PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4833
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6273
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }

    /**
     * Makes proper conversion of internal cross-coupling, bias and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param b internal bias matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b, final Matrix g) throws AlgebraException {
        setResult(m, b);

        // Gg = M*G
        m.multiply(g, estimatedGg);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param b internal bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix b) throws AlgebraException {
        // Because:
        // M = I + Mg
        // b = M^-1*bg

        // Then:
        // Mg = M - I
        // bg = M*b

        if (estimatedBiases == null) {
            estimatedBiases = new double[BodyKinematics.COMPONENTS];
        }

        final var bg = m.multiplyAndReturnNew(b);
        bg.toArray(estimatedBiases);

        if (estimatedMg == null) {
            estimatedMg = m;
        } else {
            estimatedMg.copyFrom(m);
        }

        for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
            estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
        }

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 343
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 343
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 345
final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 537
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3621
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3585
public void setListener(final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return magneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     * If not provided a default model will be loaded internally.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.magneticModel = magneticModel;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | IOException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 929
final var myz = unknowns.getElementAtIndex(5);

        fillMa(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateGeneral() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        // [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        // [fmeasy]   [by]     [myx    1+sy    myz ][ftruey]
        // [fmeasz]   [bz]     [mzx    mzy     1+sz][ftruez]

        // fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        // fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
        // fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0       0       0       0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruex  ftruez  0       0     ][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0       0       ftruex  ftruey][sz ]   [fmeasz - ftruez - bz]
        //                                                                         [mxy]
        //                                                                         [mxz]
        //                                                                         [myx]
        //                                                                         [myz]
        //                                                                         [mzx]
        //                                                                         [mzy]

        final var expectedKinematics = new BodyKinematics();

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, GENERAL_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 703
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 707
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4746
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5782
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6297
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 184
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 189
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 186
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 188
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5478
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10599
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 11105
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6216
}

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final var velocity = new ECEFVelocity();
        final var result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(), 0.0, 0.0, 0.0,
                result, velocity);
        return result;
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param value angular speed value.
     * @param unit  unit of angular speed value.
     * @return converted value.
     */
    private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
        return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Converts angular speed instance to radians per second (rad/s).
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value.
     */
    private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
        return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }

    /**
     * Converts time instance to seconds.
     *
     * @param time time instance to be converted.
     * @return converted value.
     */
    private static double convertTime(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
    }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 909
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 750
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 735
listener.onCalibrateProgressChange(RANSACRobustEasyGyroscopeCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5141
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6178
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5219
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5373
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6256
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6410
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5659
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6676
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5738
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5890
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6755
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6907
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 342
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 341
RANSACRobustKnownFrameMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1061
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1060
RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 2860
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 2352
final PreliminaryResult preliminaryResult) {
        // The magnetometer model is:
        // mBmeas = ba + (I + Mm) * mBtrue

        // Hence:
        // [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz]) [mBtruex]
        // [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]  [mBtruey]
        // [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]  [mBtruez]

        final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
        final var ecefFrame = measurement.getFrame();

        final var nedFrame = ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(ecefFrame);
        final var year = measurement.getYear();

        final var latitude = nedFrame.getLatitude();
        final var longitude = nedFrame.getLongitude();
        final var height = nedFrame.getHeight();

        final var earthB = wmmEstimator.estimate(latitude, longitude, height, year);

        final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        nedFrame.getCoordinateTransformation(cbn);
        cbn.inverse(cnb);

        final var expectedMagneticFluxDensity = BodyMagneticFluxDensityEstimator.estimate(earthB, cnb);

        final var bMeasX1 = measuredMagneticFluxDensity.getBx();
        final var bMeasY1 = measuredMagneticFluxDensity.getBy();
        final var bMeasZ1 = measuredMagneticFluxDensity.getBz();

        final var bTrueX = expectedMagneticFluxDensity.getBx();
        final var bTrueY = expectedMagneticFluxDensity.getBy();
        final var bTrueZ = expectedMagneticFluxDensity.getBz();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 339
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 343
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 343
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 345
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<PreliminaryResult>(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1369
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1500
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
            a.setElementAt(i, 3, omegaTrueY);
            a.setElementAt(i, 4, omegaTrueZ);
            a.setElementAt(i, 6, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10515
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 11018
innerCalibrator.calibrate();

            result.estimatedMg = innerCalibrator.getEstimatedMg();
            result.estimatedGg = innerCalibrator.getEstimatedGg();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setTurntableRotationRate(turntableRotationRate);
                innerCalibrator.setTimeInterval(timeInterval);
                innerCalibrator.setGDependentCrossBiasesEstimated(estimateGDependentCrossBiases);
                innerCalibrator.setInitialMg(preliminaryResult.estimatedMg);
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1305
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 715
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz) {
        return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 344
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 346
super(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(
                new PROMedSRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4871
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5908
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5004
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6041
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6811
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7939
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5399
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6416
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5525
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6542
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7301
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8409
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3967
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4715
initial[3] = initialSx;
                initial[4] = initialSy;
                initial[5] = initialSz;

                initial[6] = initialMxy;
                initial[7] = initialMxz;
                initial[8] = initialMyx;
                initial[9] = initialMyz;
                initial[10] = initialMzx;
                initial[11] = initialMzy;

                return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final var bx = params[0];
                final var by = params[1];
                final var bz = params[2];

                final var sx = params[3];
                final var sy = params[4];
                final var sz = params[5];

                final var mxy = params[6];
                final var mxz = params[7];
                final var myx = params[8];
                final var myz = params[9];
                final var mzx = params[10];
                final var mzy = params[11];

                final var ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7470
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7634
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7949
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8110
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1345
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1455
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
            a.setElementAt(i, 1, 0.0);
            a.setElementAt(i, 2, 0.0);
            a.setElementAt(i, 3, fTrueY);
            a.setElementAt(i, 4, fTrueZ);
            a.setElementAt(i, 5, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2681
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2903
public void setListener(final KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 346
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6881
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7017
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7232
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8360
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8009
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8145
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7369
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7499
final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7710
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8820
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8477
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8607
final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3204
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4134
return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS];

                initial[0] = initialSx;
                initial[1] = initialSy;
                initial[2] = initialSz;

                initial[3] = initialMxy;
                initial[4] = initialMxz;
                initial[5] = initialMyx;
                initial[6] = initialMyz;
                initial[7] = initialMzx;
                initial[8] = initialMzy;

                return initial;
            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that:
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters sx, sy, sz,
                // mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final var sx = params[0];
                final var sy = params[1];
                final var sz = params[2];

                final var mxy = params[3];
                final var mxz = params[4];
                final var myx = params[5];
                final var myz = params[6];
                final var mzx = params[7];
                final var mzy = params[8];

                final var ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 704
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 185
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 190
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 187
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 857
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 969
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 916
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 340
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 978
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 925
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 863
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1796
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 345
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1828
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1058
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1055
RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3963
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5313
setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];
        final var m21 = result[4];
        final var m31 = result[5];

        final var m12 = result[6];
        final var m22 = result[7];
        final var m32 = result[8];

        final var m13 = result[9];
        final var m23 = result[10];
        final var m33 = result[11];

        final var g11 = result[12];
        final var g21 = result[13];
        final var g31 = result[14];

        final var g12 = result[15];
        final var g22 = result[16];
        final var g32 = result[17];

        final var g13 = result[18];
        final var g23 = result[19];
        final var g33 = result[20];

        final var b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 708
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 189
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 909
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6631
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6748
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias, final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6947
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8075
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7087
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8215
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7089
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7234
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7759
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7877
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8217
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8362
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7124
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7240
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7432
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8540
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7567
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8675
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7569
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7712
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8232
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8348
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8677
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8822
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1058
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2276
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1194
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2138
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();

            a.setElementAt(i, 0, 1.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 340
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 704
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 814
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 185
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 823
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 597
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 708
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 581
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 190
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 187
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 909
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1955
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1943
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1948
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1130
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 600
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
        return estimateKinematicsAndReturnNew(timeInterval, frame, oldFrame);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
            final double x, final double y, final double z) {
        return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param position     cartesian body position resolved along ECEF-frame axes.
     * @return a new body kinematics instance.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public BodyKinematics estimateAndReturnNew(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1676
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1695
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3399
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3464
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1955
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 340
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
super(measurements, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<PreliminaryResult>(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5636
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6156
estimatedMa = preliminaryResult.estimatedMa;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }

    /**
     * Computes gravity norm for current position.
     *
     * @return gravity norm for current position expressed in meters per squared second.
     */
    protected double computeGravityNorm() {
        final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
                position.getX(), position.getY(), position.getZ());
        return gravity.getNorm();
    }

    /**
     * Converts provided NED position expressed in terms of latitude, longitude and height respect
     * mean Earth surface, to position expressed in ECEF coordinates.
     *
     * @param position NED position to be converted.
     * @return converted position expressed in ECEF coordinates.
     */
    private static ECEFPosition convertPosition(final NEDPosition position) {
        final var velocity = new ECEFVelocity();
        final var result = new ECEFPosition();
        NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                position.getLatitude(), position.getLongitude(), position.getHeight(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }

    /**
     * Converts acceleration value and unit to meters per squared second.
     *
     * @param value acceleration value.
     * @param unit  unit of acceleration value.
     * @return converted value.
     */
    private static double convertAcceleration(final double value, final AccelerationUnit unit) {
        return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated accelerometer scale factors and cross coupling errors.
         * This is the product of matrix Ta containing cross coupling errors and Ka
         * containing scaling factors.
         * So tat:
         * <pre>
         *     Ma = [sx    mxy  mxz] = Ta*Ka
         *          [myx   sy   myz]
         *          [mzx   mzy  sz ]
         * </pre>
         * Where:
         * <pre>
         *     Ka = [sx 0   0 ]
         *          [0  sy  0 ]
         *          [0  0   sz]
         * </pre>
         * and
         * <pre>
         *     Ta = [1          -alphaXy    alphaXz ]
         *          [alphaYx    1           -alphaYz]
         *          [-alphaZx   alphaZy     1       ]
         * </pre>
         * Hence:
         * <pre>
         *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
         *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
         *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
         * </pre>
         * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
         * are considered to be zero if the accelerometer z-axis is assumed to be the same
         * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
         * becomes upper diagonal:
         * <pre>
         *     Ma = [sx    mxy  mxz]
         *          [0     sy   myz]
         *          [0     0    sz ]
         * </pre>
         * Values of this matrix are unit-less.
         */
        private Matrix estimatedMa;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3796
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4436
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5115
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5799
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);

        jacobian.setElementAt(0, 0, m11);
        jacobian.setElementAt(0, 1, m12);
        jacobian.setElementAt(0, 2, m13);
        jacobian.setElementAt(0, 3, bx);
        jacobian.setElementAt(0, 4, by);
        jacobian.setElementAt(0, 6, bz);

        jacobian.setElementAt(1, 1, m22);
        jacobian.setElementAt(1, 2, m23);
        jacobian.setElementAt(1, 5, by);
        jacobian.setElementAt(1, 7, bz);

        jacobian.setElementAt(2, 2, m33);
        jacobian.setElementAt(2, 8, bz);

        jacobian.setElementAt(3, 3, 1.0);
        jacobian.setElementAt(4, 5, 1.0);
        jacobian.setElementAt(5, 8, 1.0);

        jacobian.setElementAt(6, 4, 1.0);
        jacobian.setElementAt(7, 6, 1.0);

        jacobian.setElementAt(9, 7, 1.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 727
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 712
listener.onCalibrateProgressChange(LMedSRobustEasyGyroscopeCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5885
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5403
innerCalibrator.getEstimatedBiases(result.estimatedBiases);
            result.estimatedMg = innerCalibrator.getEstimatedMg();
            result.estimatedGg = innerCalibrator.getEstimatedGg();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = sequences.size();

            final var inlierSequences = new ArrayList<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierSequences.add(sequences.get(i));
                }
            }

            try {
                innerCalibrator.setGDependentCrossBiasesEstimated(estimateGDependentCrossBiases);
                innerCalibrator.setInitialBias(preliminaryResult.estimatedBiases);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7470
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8599
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8763
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7949
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9059
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9220
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 318
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 317
LMedSRobustKnownFrameMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            setupWmmEstimator();

            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1035
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1035
LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this,
                            progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            initialize();

            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 814
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 759
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 823
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 768
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4909
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6349
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
            throws EvaluationException {

        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];

        final var g11 = params[12];
        final var g21 = params[13];
        final var g31 = params[14];

        final var g12 = params[15];
        final var g22 = params[16];
        final var g32 = params[17];

        final var g13 = params[18];
        final var g23 = params[19];
        final var g33 = params[20];

        return evaluate(i, bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3086
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4381
b = invInitM.multiplyAndReturnNew(bg);

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Before and after normalized gravity versors
                return 2 * BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES];

                // upper diagonal cross coupling errors M
                var k = 0;
                for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                // g-dependent cross biases G
                final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                for (int i = 0, j = k; i < num; i++, j++) {
                    initial[j] = initG.getElementAtIndex(i);
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1221
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1225
this(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm);
        this.listener = listener;
    }

    /**
     * Gets ground truth magnetic flux density norm to be expected at location where measurements have been made,
     * expressed in Teslas (T).
     *
     * @return ground truth magnetic flux density or null.
     */
    public Double getGroundTruthMagneticFluxDensityNorm() {
        return groundTruthMagneticFluxDensityNorm;
    }

    /**
     * Gets ground truth magnetic flux density norm to be expected at location where measurements have been made.
     *
     * @return ground truth magnetic flux density or null.
     */
    public MagneticFluxDensity getGroundTruthMagneticFluxDensityNormAsMagneticFluxDensity() {
        return groundTruthMagneticFluxDensityNorm != null
                ? new MagneticFluxDensity(groundTruthMagneticFluxDensityNorm, MagneticFluxDensityUnit.TESLA) : null;
    }

    /**
     * Gets ground truth magnetic flux density norm to be expected at location where measurements have been made.
     *
     * @param result instance where result will be stored.
     * @return true if ground truth magnetic flux density norm has been defined, false if it is not available yet.
     */
    public boolean getGroundTruthMagneticFluxDensityNormAsMagneticFluxDensity(final MagneticFluxDensity result) {
        if (groundTruthMagneticFluxDensityNorm != null) {
            result.setValue(groundTruthMagneticFluxDensityNorm);
            result.setUnit(MagneticFluxDensityUnit.TESLA);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made, expressed in Teslas (T).
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm)
            throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }

        internalSetGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
    }

    /**
     * Sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthMagneticFluxDensityNorm(final MagneticFluxDensity groundTruthMagneticFluxDensityNorm)
            throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        if (groundTruthMagneticFluxDensityNorm != null) {
            internalSetGroundTruthMagneticFluxDensityNorm(MagneticFluxDensityConverter.convert(
                    groundTruthMagneticFluxDensityNorm.getValue().doubleValue(),
                    groundTruthMagneticFluxDensityNorm.getUnit(),
                    MagneticFluxDensityUnit.TESLA));
        } else {
            internalSetGroundTruthMagneticFluxDensityNorm(null);
        }
    }

    /**
     * Gets known x-coordinate of magnetometer hard-iron bias.
     * This is expressed in Teslas (T).
     *
     * @return known x-coordinate of magnetometer hard-iron bias.
     */
    @Override
    public double getHardIronX() {
File Line
com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java 107
com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java 142
protected AccumulatedMeasurementNoiseEstimator(final L listener) {
        this.listener = listener;
    }

    /**
     * Gets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @return time interval between accelerometer triad samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @return time interval between accelerometer triad samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between accelerometer triad samples.
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(),
                timeInterval.getUnit(), TimeUnit.SECOND));
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public L getListener() {
        return listener;
    }

    /**
     * Sets listener to handle events raised by this estimator.
     *
     * @param listener listener to handle events raised by this estimator.
     * @throws LockedException if this estimator is running.
     */
    public void setListener(final L listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets last provided measurement or null if not available.
     *
     * @return last provided measurement or null.
     */
    public M getLastMeasurement() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4683
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5719
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5220
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6237
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 552
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 552
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2157
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2156
setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1624
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 967
final var specificForceZ = invAveCbe.getElementAtIndex(2);

                // save result data
                result.setSpecificForceCoordinates(specificForceX, specificForceY, specificForceZ);
                result.setAngularRateCoordinates(angularRateX, angularRateY, angularRateZ);

            } catch (final AlgebraException ignore) {
                // never happens
            }
        } else {
            // If time interval is zero, set angular rate and specific force to zero
            result.setSpecificForceCoordinates(0.0, 0.0, 0.0);
            result.setAngularRateCoordinates(0.0, 0.0, 0.0);
        }
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
            final double vx, final double vy, final double vz,
            final double oldVx, final double oldVy, final double oldVz,
            final double x, final double y, final double z, final BodyKinematics result) {
        estimateKinematics(
                TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
                c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            boy-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param velocity     velocity of body frame with respect ECEF frame.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param x            cartesian x coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param y            cartesian y coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param z            cartesian z coordinate of body position expressed in meters (m)
     *                     with respect ECEF frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3162
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4461
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5007
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, 0.0);
        mm.setElementAtIndex(2, 0.0);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, 0.0);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mg);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3433
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4760
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5352
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, m21);
        mm.setElementAtIndex(2, m31);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, m32);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mg);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5295
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5448
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6332
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6485
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5816
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6833
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6983
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4397
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4934
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                // g-dependent cross biases G
                final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                for (int i = 0, j = k; i < num; i++, j++) {
                    initial[j] = initG.getElementAtIndex(i);
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {

                measAngularRateX = point[0];
                measAngularRateY = point[1];
                measAngularRateZ = point[2];

                fmeasX = point[3];
                fmeasY = point[4];
                fmeasZ = point[5];

                gradientEstimator.gradient(params, derivatives);

                return evaluateCommonAxisWitGDependentCrossBiases(params);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1414
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1385
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 750
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 735
PROSACRobustEasyGyroscopeCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4873
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5072
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6109
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5784
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5910
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5282
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5401
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5594
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6611
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6299
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6418
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 538
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 538
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 342
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 341
PROSACRobustKnownFrameMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2143
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2142
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1061
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1060
PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 832
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 945
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 892
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 316
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 953
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 899
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 839
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1772
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 321
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1805
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1034
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1031
LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1576
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 340
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1594
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 346
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 678
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 665
listener.onCalibrateProgressChange(MSACRobustEasyGyroscopeCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            setupAccelerationFixer();

            inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4524
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4627
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4808
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5845
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4937
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5974
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4939
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5074
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5560
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5663
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position,
                    turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5976
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6111
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5058
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5163
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5339
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6356
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5461
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6478
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5463
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5596
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6075
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6180
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6480
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6613
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 271
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 270
MSACRobustKnownFrameMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            setupWmmEstimator();

            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 987
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 988
MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;

            initialize();

            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13634
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6391
private static double convertTimeToDouble(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Converts provided distance instance into its corresponding value expressed in
     * meters.
     *
     * @param distance distance instance to be converted.
     * @return converted value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance into its corresponding value expressed in
     * meters per second.
     *
     * @param speed speed instance to be converted.
     * @return converted value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Converts provided acceleration instance into its corresponding value expressed
     * in meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value expressed in meters per squared second.
     */
    private static double convertAccelerationToDouble(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts provided angular speed into its corresponding value expressed in
     * radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value expressed in radians per second.
     */
    private static double convertAngularSpeedToDouble(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }
}
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1952
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 2197
public boolean equals(final INSLooselyCoupledKalmanState other, final double threshold) {
        if (other == null) {
            return false;
        }

        return Math.abs(vx - other.vx) <= threshold
                && Math.abs(vy - other.vy) <= threshold
                && Math.abs(vz - other.vz) <= threshold
                && Math.abs(x - other.x) <= threshold
                && Math.abs(y - other.y) <= threshold
                && Math.abs(z - other.z) <= threshold
                && Math.abs(accelerationBiasX - other.accelerationBiasX) <= threshold
                && Math.abs(accelerationBiasY - other.accelerationBiasY) <= threshold
                && Math.abs(accelerationBiasZ - other.accelerationBiasZ) <= threshold
                && Math.abs(gyroBiasX - other.gyroBiasX) <= threshold
                && Math.abs(gyroBiasY - other.gyroBiasY) <= threshold
                && Math.abs(gyroBiasZ - other.gyroBiasZ) <= threshold
                && other.bodyToEcefCoordinateTransformationMatrix != null
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 814
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 768
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 759
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 823
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4883
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4982
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4404
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4502
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7389
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8683
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7554
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8517
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7866
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9139
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8029
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8976
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias,
                    initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.*
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 405
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1285
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1580
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1255
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 409
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 408
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2010
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5787
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6563
ftrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (tmp == null) {
                tmp = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            measAngularRate.setElementAtIndex(0, measAngularRateX);
            measAngularRate.setElementAtIndex(1, measAngularRateY);
            measAngularRate.setElementAtIndex(2, measAngularRateZ);

            fmeas.setElementAtIndex(0, fmeasX);
            fmeas.setElementAtIndex(1, fmeasY);
            fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, biasX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6747
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7875
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6813
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6949
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7941
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8077
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7238
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8346
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7304
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7434
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8412
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8542
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1704
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1869
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1773
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 535
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1795
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 857
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 969
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 916
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 340
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 978
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 925
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1710
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3595
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 539
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3658
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 863
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1796
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 345
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1828
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2141
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2138
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1058
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1055
PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 704
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 759
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 185
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 768
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 597
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 708
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 581
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 190
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 187
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 909
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7161
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8436
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7308
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8289
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7640
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8894
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7784
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8748
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java 182
com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java 202
reset();
    }

    /**
     * Gets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @return time interval between accelerometer triad samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @return time interval between accelerometer triad samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between accelerometer triad samples.
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
                TimeUnit.SECOND));
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public L getListener() {
        return listener;
    }

    /**
     * Sets listener to handle events raised by this estimator.
     *
     * @param listener listener to handle events raised by this estimator.
     * @throws LockedException if this estimator is running.
     */
    public void setListener(final L listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets first provided measurement value expressed in its default units.
     * (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
     *
     * @return first provided measurement value or null if not available.
     */
    public Double getFirstWindowedMeasurementValue() {
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 205
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 231
com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java 234
final var biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
        this.bias.setElementAtIndex(0, biasX);
        this.bias.setElementAtIndex(1, biasY);
        this.bias.setElementAtIndex(2, biasZ);
    }

    /**
     * Gets x-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @return x-coordinate of bias expressed in meters per squared
     * second (m/s^2).
     */
    public double getBiasX() {
        return bias.getElementAtIndex(0);
    }

    /**
     * Sets x-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasX x-coordinate of bias expressed in meters per squared
     *              second (m/s^2).
     */
    public void setBiasX(final double biasX) {
        bias.setElementAtIndex(0, biasX);
    }

    /**
     * Gets y-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @return y-coordinate of bias expressed in meters per squared
     * second (m/s^2).
     */
    public double getBiasY() {
        return bias.getElementAtIndex(1);
    }

    /**
     * Sets y-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasY y-coordinate of bias expressed in meters per squared
     *              second (m/s^2).
     */
    public void setBiasY(final double biasY) {
        bias.setElementAtIndex(1, biasY);
    }

    /**
     * Gets z-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @return z-coordinate of bias expressed in meters per squared
     * second (m/s^2).
     */
    public double getBiasZ() {
        return bias.getElementAtIndex(2);
    }

    /**
     * Sets z-coordinate of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasZ z-coordinate of bias expressed in meters per squared
     *              second (m/s^2).
     */
    public void setBiasZ(final double biasZ) {
        bias.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets coordinates of bias expressed in meters per squared second
     * (m/s^2).
     *
     * @param biasX x-coordinate of bias.
     * @param biasY y-coordinate of bias.
     * @param biasZ z-coordinate of bias.
     */
    public void setBias(final double biasX, final double biasY, final double biasZ) {
        setBiasX(biasX);
        setBiasY(biasY);
        setBiasZ(biasZ);
    }

    /**
     * Gets x-coordinate of bias.
     *
     * @return x-coordinate of bias.
     */
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5817
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6593
b.setElementAtIndex(2, biasZ);

            g.setElementAt(0, 0, g11);
            g.setElementAt(1, 0, g21);
            g.setElementAt(2, 0, g31);

            g.setElementAt(0, 1, g12);
            g.setElementAt(1, 1, g22);
            g.setElementAt(2, 1, g32);

            g.setElementAt(0, 2, g13);
            g.setElementAt(1, 2, g23);
            g.setElementAt(2, 2, g33);

            getAccelerometerBiasAsMatrix(ba);
            getAccelerometerMa(ma);

            // fix measured accelerometer value to obtain true
            // specific force
            accelerationFixer.fix(fmeas, ftrue);
            g.multiply(ftrue, tmp);

            invM.multiply(measAngularRate, trueAngularRate);
            trueAngularRate.subtract(b);
            trueAngularRate.subtract(tmp);

            final var norm = Utils.normF(trueAngularRate);
            return norm * norm;

        } catch (final AlgebraException e) {
            throw new EvaluationException(e);
        }
    }

    /**
     * Computes estimated true angular rate squared norm using current measured
     * angular rate and provided parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fittin needed for calibration computation.
     *
     * @param m11 element 1,1 of cross-coupling error matrix.
     * @param m21 element 2,1 of cross-coupling error matrix.
     * @param m31 element 3,1 of cross-coupling error matrix.
     * @param m12 element 1,2 of cross-coupling error matrix.
     * @param m22 element 2,2 of cross-coupling error matrix.
     * @param m32 element 3,2 of cross-coupling error matrix.
     * @param m13 element 1,3 of cross-coupling error matrix.
     * @param m23 element 2,3 of cross-coupling error matrix.
     * @param m33 element 3,3 of cross-coupling error matrix.
     * @return estimated true angular rate squared norm.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluate(final double m11, final double m21, final double m31,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6630
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7758
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7122
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8230
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 783
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 898
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 846
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 268
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 905
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 852
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 794
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1725
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 274
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1757
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 990
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 983
MSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
                }
            }
        });

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1615
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 452
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2060
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1782
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1801
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1623
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3505
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 450
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3570
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 450
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2051
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2048
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2053
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1636
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 471
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2076
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1799
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 465
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1819
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1640
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3525
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 469
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3588
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 468
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2070
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2068
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2072
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 1115
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 1117
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 6555
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 7061
}

    /**
     * Converts a time instance expressed in milliseconds since epoch time
     * (January 1st, 1970 at midnight) to a decimal year.
     *
     * @param timestampMillis milliseconds value to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Long timestampMillis) {
        if (timestampMillis == null) {
            return null;
        }

        final var calendar = new GregorianCalendar();
        calendar.setTimeInMillis(timestampMillis);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained ina date object to a
     * decimal year.
     *
     * @param date a time instance to be converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final Date date) {
        if (date == null) {
            return null;
        }

        final var calendar = new GregorianCalendar();
        calendar.setTime(date);
        return convertTime(calendar);
    }

    /**
     * Converts a time instant contained in a gregorian calendar to a
     * decimal year.
     *
     * @param calendar calendar containing a specific instant to be
     *                 converted.
     * @return converted value expressed in decimal years.
     */
    private static Double convertTime(final GregorianCalendar calendar) {
        if (calendar == null) {
            return null;
        }

        return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
    }

    /**
     * Converts provided ECEF position to position expressed in NED
     * coordinates.
     *
     * @param position ECEF position to be converted.
     * @return converted position expressed in NED coordinates.
     */
    private static NEDPosition convertPosition(final ECEFPosition position) {
        final var velocity = new NEDVelocity();
        final var result = new NEDPosition();
        ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                position.getX(), position.getY(), position.getZ(),
                0.0, 0.0, 0.0, result, velocity);
        return result;
    }
File Line
com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java 144
com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java 203
}

    /**
     * Gets time interval between triad samples expressed in
     * seconds (s).
     *
     * @return time interval between accelerometer triad samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between triad samples expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between triad samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between triad samples.
     *
     * @return time interval between triad samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between triad samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between triad samples.
     *
     * @param timeInterval time interval between triad samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
                TimeUnit.SECOND));
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public L getListener() {
        return listener;
    }

    /**
     * Sets listener to handle events raised by this estimator.
     *
     * @param listener listener to handle events raised by this estimator.
     * @throws LockedException if this estimator is running.
     */
    public void setListener(final L listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets last provided triad values or null if not available.
     *
     * @return last provided triad values or null.
     */
    public T getLastTriad() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 535
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 533
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2144
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2140
setupWmmEstimator();

            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return true;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * This method is used internally and does not check whether instance is
     * locked or not.
     *
     * @param qualityScores quality scores to be set.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    private void internalSetQualityScores(final double[] qualityScores) {
        if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
File Line
com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java 109
com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java 144
com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java 183
com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java 203
}

    /**
     * Gets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @return time interval between accelerometer triad samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @return time interval between accelerometer triad samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between accelerometer triad samples.
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(),
                timeInterval.getUnit(), TimeUnit.SECOND));
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public L getListener() {
        return listener;
    }

    /**
     * Sets listener to handle events raised by this estimator.
     *
     * @param listener listener to handle events raised by this estimator.
     * @throws LockedException if this estimator is running.
     */
    public void setListener(final L listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets last provided measurement or null if not available.
     *
     * @return last provided measurement or null.
     */
    public M getLastMeasurement() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6881
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8145
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7017
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8009
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7369
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8607
final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7499
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8477
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3025
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3158
}

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanEpochEstimator.java 704
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java 185
INSLooselyCoupledKalmanState.NUM_PARAMS);

        final var gyroNoisePSD = config.getGyroNoisePSD();
        final var gyroNoiseValue = gyroNoisePSD * propagationInterval;
        for (var i = 0; i < 3; i++) {
            qPrimeMatrix.setElementAt(i, i, gyroNoiseValue);
        }

        final var accelNoisePSD = config.getAccelerometerNoisePSD();
        final var accelNoiseValue = accelNoisePSD * propagationInterval;
        for (var i = 3; i < 6; i++) {
            qPrimeMatrix.setElementAt(i, i, accelNoiseValue);
        }

        final var accelBiasPSD = config.getAccelerometerBiasPSD();
        final var accelBiasValue = accelBiasPSD * propagationInterval;
        for (var i = 9; i < 12; i++) {
            qPrimeMatrix.setElementAt(i, i, accelBiasValue);
        }

        final var gyroBiasPSD = config.getGyroBiasPSD();
        final var gyroBiasValue = gyroBiasPSD * propagationInterval;
        for (var i = 12; i < 15; i++) {
            qPrimeMatrix.setElementAt(i, i, gyroBiasValue);
        }

        // 3. Propagate state estimates using (3.14) noting that all states are zero
        // due to closed-loop correction.
        // x_est_propagated(1:15, 1) = 0

        // 4. Propagate state estimation error covariance matrix using (3.46)
        final var pMatrixOld = previousState.getCovariance();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1060
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1196
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4925
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2140
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2278
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4317
var i = 0;
        for (final var measurement : measurements) {
            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();

            // estimate Earth magnetic flux density at frame position and
            // timestamp using WMM
            final var ecefFrame = measurement.getFrame();
            ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);

            final var year = measurement.getYear();

            final var latitude = nedFrame.getLatitude();
            final var longitude = nedFrame.getLongitude();
            final var height = nedFrame.getHeight();

            nedFrame.getCoordinateTransformation(cbn);
            cbn.inverse(cnb);

            wmmEstimator.estimate(latitude, longitude, height, year, earthB);

            // estimate expected body magnetic flux density taking into
            // account body attitude (inverse of frame orientation) and
            // estimated Earth magnetic flux density
            BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);

            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();

            final var bTrueX = expectedMagneticFluxDensity.getBx();
            final var bTrueY = expectedMagneticFluxDensity.getBy();
            final var bTrueZ = expectedMagneticFluxDensity.getBz();
File Line
com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java 212
com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java 343
result.setUnit(lastMeasurement.getUnit());
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets estimated average of measurement expressed in its default unit
     * (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
     *
     * @return average of measurement in current window.
     */
    public double getAvg() {
        return avg;
    }

    /**
     * Gets estimated average of measurement within current window.
     *
     * @return average of measurement in current window
     */
    public M getAvgAsMeasurement() {
        return createMeasurement(avg, getDefaultUnit());
    }

    /**
     * Gets estimated average of measurement within current window.
     *
     * @param result instance where average of measurement will be stored.
     */
    public void getAvgAsMeasurement(final M result) {
        result.setValue(avg);
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets estimated variance of measurement within current window
     * expressed in its default squared unit (m^2/s^4 for acceleration,
     * rad^2/s^2 for angular speed or T^2 for magnetic flux density).
     *
     * @return estimated variance of measurement within current window.
     */
    public double getVariance() {
        return variance;
    }

    /**
     * Gets estimated standard deviation of measurement within current window
     * and expressed in its default unit (m/s^2 for acceleration, rad/s for
     * angular speed or T for magnetic flux density).
     *
     * @return estimated standard of measurement.
     */
    public double getStandardDeviation() {
        return Math.sqrt(variance);
    }

    /**
     * Gets estimated standard deviation of measurement within current window.
     *
     * @return estimated standard deviation of measurement.
     */
    public M getStandardDeviationAsMeasurement() {
        return createMeasurement(getStandardDeviation(), getDefaultUnit());
    }

    /**
     * Gets estimated standard deviation of measurement within current window.
     *
     * @param result instance where estimated standard deviation of measurement
     *               will be stored.
     */
    public void getStandardDeviationAsMeasurement(final M result) {
        result.setValue(getStandardDeviation());
        result.setUnit(getDefaultUnit());
    }

    /**
     * Gets measurement noise PSD (Power Spectral Density) expressed
     * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
     * magnetometer.
     *
     * @return measurement noise PSD.
     */
    public double getPsd() {
        return variance * timeInterval;
    }

    /**
     * Gets measurement noise root PSD (Power Spectral Density) expressed in
     * (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
     * magnetometer.
     *
     * @return measurement noise root PSD.
     */
    public double getRootPsd() {
        return Math.sqrt(getPsd());
    }

    /**
     * Gets number of samples that have been processed so far.
     *
     * @return number of samples that have been processed so far.
     */
    public int getNumberOfProcessedSamples() {
        return numberOfProcessedSamples;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    public boolean isRunning() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6039
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5633
final var initialB = invInitialM.multiplyAndReturnNew(initialBa);

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are measured specific force coordinates
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                // biases b
                for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                    initial[i] = initialB.getElementAtIndex(i);
                }

                // upper diagonal cross coupling errors M
                var k = BodyKinematics.COMPONENTS;
                for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 3025
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3112
result.estimatedMa = nonLinearCalibrator.getEstimatedMa();

                if (keepCovariance) {
                    result.covariance = nonLinearCalibrator.getEstimatedCovariance();
                } else {
                    result.covariance = null;
                }

                result.estimatedMse = nonLinearCalibrator.getEstimatedMse();
                result.estimatedChiSq = nonLinearCalibrator.getEstimatedChiSq();
            }

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationFrameBodyKinematics>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                nonLinearCalibrator.setInitialBias(preliminaryResult.estimatedBiases);
                nonLinearCalibrator.setInitialMa(preliminaryResult.estimatedMa);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4087
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4174
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3597
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3684
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5219
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6410
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must
     *                                      have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5373
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6256
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This must be
     *                                      3x1 and is expressed in radians
     *                                      per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5738
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6907
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5890
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6755
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must
     *                                      be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5819
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5569
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6364
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6087
innerCalibrator.calibrate();

            result.estimatedMa = innerCalibrator.getEstimatedMa();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4626
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5662
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4685
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4810
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5721
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5847
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5162
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6179
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5222
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5341
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6239
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6358
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 7493
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 8020
innerCalibrator.calibrate();

            result.estimatedMm = innerCalibrator.getEstimatedMm();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setHardIronCoordinates(hardIronX, hardIronY, hardIronZ);
File Line
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1110
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1012
public void setListener(final INSGNSSTightlyCoupledKalmanFilteredEstimatorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum epoch interval expressed in seconds (s) between consecutive
     * propagations or measurements expressed in seconds.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @return minimum epoch interval between consecutive propagations or
     * measurements.
     */
    public double getEpochInterval() {
        return epochInterval;
    }

    /**
     * Sets minimum epoch interval expressed in seconds (s) between consecutive
     * propagations or measurements expressed in seconds.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param epochInterval minimum epoch interval expressed in seconds (s) between
     *                      consecutive propagations or measurements.
     * @throws LockedException          if this estimator is already running.
     * @throws IllegalArgumentException if provided epoch interval is negative.
     */
    public void setEpochInterval(final double epochInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (epochInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.epochInterval = epochInterval;
    }

    /**
     * Gets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param result instance where minimum epoch interval will be stored.
     */
    public void getEpochIntervalAsTime(final Time result) {
        result.setValue(epochInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Gets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @return minimum epoch interval.
     */
    public Time getEpochIntervalAsTime() {
        return new Time(epochInterval, TimeUnit.SECOND);
    }

    /**
     * Sets minimum epoch interval between consecutive propagations or measurements.
     * Attempting to propagate results using Kalman filter or updating measurements
     * when intervals are less than this value, will be ignored.
     *
     * @param epochInterval minimum epoch interval.
     * @throws LockedException          if this estimator is already running.
     * @throws IllegalArgumentException if provided epoch interval is negative.
     */
    public void setEpochInterval(final Time epochInterval) throws LockedException {
        final var epochIntervalSeconds = TimeConverter.convert(epochInterval.getValue().doubleValue(),
                epochInterval.getUnit(), TimeUnit.SECOND);
        setEpochInterval(epochIntervalSeconds);
    }

    /**
     * Gets INS/GNSS tightly coupled Kalman configuration parameters (usually
     * obtained through calibration).
     *
     * @param result instance where INS/GNSS tightly coupled Kalman configuration
     *               parameters will be stored.
     * @return true if result instance is updated, false otherwise.
     */
    public boolean getConfig(final INSTightlyCoupledKalmanConfig result) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3652
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4971
setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];

        final var m12 = result[4];
        final var m22 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var g11 = result[9];
        final var g21 = result[10];
        final var g31 = result[11];

        final var g12 = result[12];
        final var g22 = result[13];
        final var g32 = result[14];

        final var g13 = result[15];
        final var g23 = result[16];
        final var g33 = result[17];

        final var b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1369
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1048
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1500
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 907
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, omegaTrueX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5039
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5164
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4559
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4684
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5006
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6180
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5143
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6043
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5292
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7467
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6329
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8596
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5527
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6678
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5661
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6544
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated,
     *                                      false otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5813
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7946
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6830
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9056
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2696
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3033
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5749
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5498
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6287
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6010
final var estMa = preliminaryResult.estimatedMa;

            if (identity == null) {
                identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp1 == null) {
                tmp1 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp2 == null) {
                tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp3 == null) {
                tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (tmp4 == null) {
                tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            identity.add(estMa, tmp1);

            Utils.inverse(tmp1, tmp2);

            final var kinematics = measurement.getKinematics();
            final var fmeasX = kinematics.getFx();
            final var fmeasY = kinematics.getFy();
            final var fmeasZ = kinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1819
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3835
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3795
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3847
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3882
}

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @return position where body kinematics measures have been taken.
     */
    public ECEFPosition getEcefPosition() {
        return position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * ECEF coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final var result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (position != null) {
            final var velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(position.getX(), position.getY(), position.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.position = convertPosition(position);
    }

    /**
     * Gets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @return list of body kinematics measurements.
     */
    @Override
    public List<StandardDeviationBodyKinematics> getMeasurements() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4523
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5559
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5057
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6074
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 7423
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 7943
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6907
final var estMm = preliminaryResult.estimatedMm;

            if (identity == null) {
                identity = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }

            if (tmp1 == null) {
                tmp1 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }

            if (tmp2 == null) {
                tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp3 == null) {
                tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (tmp4 == null) {
                tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            identity.add(estMm, tmp1);

            Utils.inverse(tmp1, tmp2);

            final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
            final var bMeasX = measuredMagneticFluxDensity.getBx();
            final var bMeasY = measuredMagneticFluxDensity.getBy();
            final var bMeasZ = measuredMagneticFluxDensity.getBz();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2696
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3166
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2918
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3033
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5292
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8596
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6329
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7467
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5813
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9056
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6830
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7946
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6688
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7816
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias, final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7179
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8287
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 356
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1075
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1074
setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.RANSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5910
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4873
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5784
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5445
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7631
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6482
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8760
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7089
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8362
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7234
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8217
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5282
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6418
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5401
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6299
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5963
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8107
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6980
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9217
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7569
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8822
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7712
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8677
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      measurement. The larger the score value the better
     *                                      the quality of the sample.
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2681
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 395
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2903
public void setListener(final KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 721
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 208
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 925
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 833
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 203
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 840
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 729
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1660
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 209
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1692
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 206
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 924
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 918
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 923
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 275
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 913
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 798
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1731
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 280
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1763
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 277
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 992
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 990
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 995
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 406
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3152
public void setListener(final KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6571
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7699
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6572
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6690
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7700
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7817
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7065
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8171
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7067
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7181
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8173
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8289
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
            final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1081
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 401
}

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1527
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 1062
}

    /**
     * Fills scale factor and cross coupling error matrix with estimated values.
     *
     * @param sx  x scale factor
     * @param sy  y scale factor
     * @param sz  z scale factor
     * @param mxy x-y cross coupling
     * @param mxz x-z cross coupling
     * @param myx y-x cross coupling
     * @param myz y-z cross coupling
     * @param mzx z-x cross coupling
     * @param mzy z-y cross coupling
     * @throws WrongSizeException never happens.
     */
    private void fillMa(final double sx, final double sy, final double sz,
                        final double mxy, final double mxz, final double myx,
                        final double myz, final double mzx, final double mzy) throws WrongSizeException {
        if (estimatedMa == null) {
            estimatedMa = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        }

        estimatedMa.setElementAt(0, 0, sx);
        estimatedMa.setElementAt(1, 0, myx);
        estimatedMa.setElementAt(2, 0, mzx);

        estimatedMa.setElementAt(0, 1, mxy);
        estimatedMa.setElementAt(1, 1, sy);
        estimatedMa.setElementAt(2, 1, mzy);

        estimatedMa.setElementAt(0, 2, mxz);
        estimatedMa.setElementAt(1, 2, myz);
        estimatedMa.setElementAt(2, 2, sz);
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4957
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6396
private double evaluateCommonAxisWithGDependentCrossBiases(final int i, final double[] params)
            throws EvaluationException {

        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];

        final var m12 = params[4];
        final var m22 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        final var g11 = params[9];
        final var g21 = params[10];
        final var g31 = params[11];

        final var g12 = params[12];
        final var g22 = params[13];
        final var g32 = params[14];

        final var g13 = params[15];
        final var g23 = params[16];
        final var g33 = params[17];

        return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1085
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 413
}

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated gyroscope scale factors and cross coupling errors.
     * This is the product of matrix Tg containing cross coupling errors and Kg
     * containing scaling factors.
     * So that:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Kg = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Tg = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Mg = [sx    mxy  mxz] = Tg*Kg =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the gyroscope z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
     * becomes upper diagonal:
     * <pre>
     *     Mg = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated gyroscope scale factors and cross coupling errors.
     */
    @Override
    public Matrix getEstimatedMg() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4194
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5600
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
            throws EvaluationException {

        final var m11 = params[0];
        final var m21 = params[1];
        final var m31 = params[2];

        final var m12 = params[3];
        final var m22 = params[4];
        final var m32 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        final var g11 = params[9];
        final var g21 = params[10];
        final var g31 = params[11];

        final var g12 = params[12];
        final var g22 = params[13];
        final var g32 = params[14];

        final var g13 = params[15];
        final var g23 = params[16];
        final var g33 = params[17];

        return evaluate(i, m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MidPointQuaternionStepIntegrator.java 134
com/irurueta/navigation/inertial/calibration/gyroscope/RungeKuttaQuaternionStepIntegrator.java 163
omega0, omega1, omega01, quat, quatResult, tmpQ, k1, k2, omegaSkew);
    }

    /**
     * Performs a mid-point integration step.
     * More information available here:
     * <a href="https://en.wikipedia.org/wiki/Midpoint_method">https://en.wikipedia.org/wiki/Midpoint_method</a>
     *
     * @param initialAttitude initial attitude.
     * @param initialWx       initial x-coordinate rotation velocity at initial timestamp expressed
     *                        in radians per second (rad/s).
     * @param initialWy       initial y-coordinate rotation velocity at initial timestamp expressed
     *                        in radians per second (rad/s).
     * @param initialWz       initial z-coordinate rotation velocity at initial timestamp expressed
     *                        in radians per second (rad/s).
     * @param currentWx       end x-coordinate rotation velocity at end timestamp expressed in
     *                        radians per second (rad/s).
     * @param currentWy       end y-coordinate rotation velocity at end timestamp expressed in
     *                        radians per second (rad/s).
     * @param currentWz       end z-coordinate rotation velocity at end timestamp expressed in
     *                        radians per second (rad/s).
     * @param dt              time step expressed in seconds (t1 - t0).
     * @param result          instance where result of integration will be stored.
     * @throws RotationException if a numerical error occurs.
     */
    public static void integrationStep(
            final Quaternion initialAttitude,
            final double initialWx, final double initialWy, final double initialWz,
            final double currentWx, final double currentWy, final double currentWz,
            final double dt, final Quaternion result) throws RotationException {
        try {
            final var omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
            final var omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
            final var omega01 = new Matrix(Rotation3D.INHOM_COORDS, 1);
            final var quat = new Matrix(Quaternion.N_PARAMS, 1);
            final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
            final var tmpQ = new Matrix(Quaternion.N_PARAMS, 1);
            final var k1 = new Matrix(Quaternion.N_PARAMS, 1);
            final var k2 = new Matrix(Quaternion.N_PARAMS, 1);
            final var omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5257
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10445
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            m1.add(mg);

            final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final var m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0);
            final var angularRateMeasY2 = biasY + m1.getElementAtIndex(1);
            final var angularRateMeasZ2 = biasZ + m1.getElementAtIndex(2);

            final var diffX = angularRateMeasX2 - angularRateMeasX1;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 94
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 98
KnownBiasGyroscopeCalibrator, OrderedStandardDeviationBodyKinematicsGyroscopeCalibrator,
        QualityScoredGyroscopeCalibrator, AccelerometerDependentGyroscopeCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = true;

    /**
     * Indicates that by default G-dependent cross biases introduced
     * by the accelerometer on the gyroscope are estimated.
     */
    public static final boolean DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES = true;

    /**
     * Default turntable rotation rate.
     */
    public static final double DEFAULT_TURNTABLE_ROTATION_RATE =
            TurntableGyroscopeCalibrator.DEFAULT_TURNTABLE_ROTATION_RATE;

    /**
     * Default time interval between measurements expressed in seconds (s).
     * This is a typical value when we have 50 samples per second.
     */
    public static final double DEFAULT_TIME_INTERVAL = TurntableGyroscopeCalibrator.DEFAULT_TIME_INTERVAL;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzy;

    /**
     * Known x-coordinate of gyroscope bias.
     * This is expressed in radians per second (rad/s).
     */
    private double biasX;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5445
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8760
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6482
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7631
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3039
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10944
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            m1.add(mg);

            final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final var m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final var angularRateMeasX2 = bx + m1.getElementAtIndex(0);
            final var angularRateMeasY2 = by + m1.getElementAtIndex(1);
            final var angularRateMeasZ2 = bz + m1.getElementAtIndex(2);

            final var diffX = angularRateMeasX2 - angularRateMeasX1;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5963
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9217
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6980
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8107
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1288
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2364
}

    /**
     * Fills scale factor and cross coupling error matrix with estimated values.
     *
     * @param sx  x scale factor
     * @param sy  y scale factor
     * @param sz  z scale factor
     * @param mxy x-y cross coupling
     * @param mxz x-z cross coupling
     * @param myx y-x cross coupling
     * @param myz y-z cross coupling
     * @param mzx z-x cross coupling
     * @param mzy z-y cross coupling
     * @throws WrongSizeException never happens.
     */
    private void fillMm(final double sx, final double sy, final double sz,
                        final double mxy, final double mxz, final double myx,
                        final double myz, final double mzx, final double mzy) throws WrongSizeException {
        if (estimatedMm == null) {
            estimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
        }

        estimatedMm.setElementAt(0, 0, sx);
        estimatedMm.setElementAt(1, 0, myx);
        estimatedMm.setElementAt(2, 0, mzx);

        estimatedMm.setElementAt(0, 1, mxy);
        estimatedMm.setElementAt(1, 1, sy);
        estimatedMm.setElementAt(2, 1, mzy);

        estimatedMm.setElementAt(0, 2, mxz);
        estimatedMm.setElementAt(1, 2, myz);
        estimatedMm.setElementAt(2, 2, sz);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5821
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5571
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6365
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6088
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10518
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 11020
result.estimatedMa = innerCalibrator.getEstimatedMa();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4999
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3222
listener.onCalibrateEnd((C) this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 101
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 129
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzy;

    /**
     * Initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double initialBiasX;

    /**
     * Initial y-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double initialBiasY;

    /**
     * Initial z-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double initialBiasZ;

    /**
     * Initial gyroscope x scaling factor.
     */
    private double initialSx;

    /**
     * Initial gyroscope y scaling factor.
     */
    private double initialSy;

    /**
     * Initial gyroscope z scaling factor.
     */
    private double initialSz;

    /**
     * Initial gyroscope x-y cross coupling error.
     */
    private double initialMxy;

    /**
     * Initial gyroscope x-z cross coupling error.
     */
    private double initialMxz;

    /**
     * Initial gyroscope y-x cross coupling error.
     */
    private double initialMyx;

    /**
     * Initial gyroscope y-z cross coupling error.
     */
    private double initialMyz;

    /**
     * Initial gyroscope z-x cross coupling error.
     */
    private double initialMzx;

    /**
     * Initial gyroscope z-y cross coupling error.
     */
    private double initialMzy;

    /**
     * Initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     */
    private Matrix initialGg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5212
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 2988
return create(measurements, bias, commonAxisUsed, listener, DEFAULT_ROBUST_METHOD);
    }

    /**
     * Computes error of a preliminary result respect a given measurement.
     *
     * @param measurement       a measurement.
     * @param preliminaryResult a preliminary result.
     * @return computed error.
     */
    protected double computeError(
            final StandardDeviationFrameBodyKinematics measurement, final PreliminaryResult preliminaryResult) {
        // We know that measured angular rate is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        final var measuredKinematics = measurement.getKinematics();
        final var ecefFrame = measurement.getFrame();
        final var previousEcefFrame = measurement.getPreviousFrame();
        final var timeInterval = measurement.getTimeInterval();

        final var expectedKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
                previousEcefFrame);

        final var angularRateMeasX1 = measuredKinematics.getAngularRateX();
        final var angularRateMeasY1 = measuredKinematics.getAngularRateY();
        final var angularRateMeasZ1 = measuredKinematics.getAngularRateZ();

        final var angularRateTrueX = expectedKinematics.getAngularRateX();
        final var angularRateTrueY = expectedKinematics.getAngularRateY();
        final var angularRateTrueZ = expectedKinematics.getAngularRateZ();

        final var fTrueX = expectedKinematics.getFx();
        final var fTrueY = expectedKinematics.getFy();
        final var fTrueZ = expectedKinematics.getFz();

        final var mg = preliminaryResult.estimatedMg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 96
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 124
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzy;

    /**
     * X-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double biasX;

    /**
     * Y-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double biasY;

    /**
     * Z-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double biasZ;

    /**
     * Initial gyroscope x scaling factor.
     */
    private double initialSx;

    /**
     * Initial gyroscope y scaling factor.
     */
    private double initialSy;

    /**
     * Initial gyroscope z scaling factor.
     */
    private double initialSz;

    /**
     * Initial gyroscope x-y cross coupling error.
     */
    private double initialMxy;

    /**
     * Initial gyroscope x-z cross coupling error.
     */
    private double initialMxz;

    /**
     * Initial gyroscope y-x cross coupling error.
     */
    private double initialMyx;

    /**
     * Initial gyroscope y-z cross coupling error.
     */
    private double initialMyz;

    /**
     * Initial gyroscope z-x cross coupling error.
     */
    private double initialMzx;

    /**
     * Initial gyroscope z-y cross coupling error.
     */
    private double initialMzy;

    /**
     * Initial G-dependent cross biases introduced on the gyroscope by the
     * specific forces sensed by the accelerometer.
     */
    private Matrix initialGg;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6813
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8077
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6949
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7941
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7304
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8542
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7434
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8412
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4225
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4338
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3735
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3848
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must have length 3 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5216
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7386
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6253
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8514
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6631
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7877
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7759
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5735
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7863
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6752
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8973
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7124
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8348
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7240
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8232
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1089
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1093
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5102
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5227
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4622
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4747
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/estimators/NEDPositionEstimator.java 2009
com/irurueta/navigation/inertial/estimators/NEDVelocityEstimator.java 2306
estimatePosition(timeInterval, oldPosition, oldVelocity, velocity, result);
        return result;
    }

    /**
     * Converts provided time instance to seconds (s).
     *
     * @param time time instance to be converted.
     * @return provided time value expressed in seconds.
     */
    private static double convertTimeToDouble(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Converts provided angle instance to radians (rad).
     *
     * @param angle angle instance to be converted.
     * @return provided angle value expressed in radians.
     */
    private static double convertAngleToDouble(final Angle angle) {
        return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
    }

    /**
     * Converts provided distance instance to meters (m).
     *
     * @param distance distance instance to be converted.
     * @return provided distance value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance to meters per second (m/s).
     *
     * @param speed speed instance to be converted.
     * @return provided speed value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }
}
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 1171
com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java 1253
final double measuredFx, final double measuredFy, final double measuredFz,
            final double biasX, final double biasY, final double biasZ,
            final double sx, final double sy, final double sz, final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy, final double[] result) throws AlgebraException {

        crossCouplingErrors.setElementAt(0, 0, sx);
        crossCouplingErrors.setElementAt(1, 1, sy);
        crossCouplingErrors.setElementAt(2, 2, sz);
        crossCouplingErrors.setElementAt(0, 1, mxy);
        crossCouplingErrors.setElementAt(0, 2, mxz);
        crossCouplingErrors.setElementAt(1, 0, myx);
        crossCouplingErrors.setElementAt(1, 2, myz);
        crossCouplingErrors.setElementAt(2, 0, mzx);
        crossCouplingErrors.setElementAt(2, 1, mzy);

        fix(measuredFx, measuredFy, measuredFz, biasX, biasY, biasZ, crossCouplingErrors, result);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1089
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 421
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets estimated accelerometer scale factors and ross coupling errors.
     * This is the product of matrix Ta containing cross coupling errors and Ka
     * containing scaling factors.
     * So tat:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka
     *          [myx   sy   myz]
     *          [mzx   mzy  sz ]
     * </pre>
     * Where:
     * <pre>
     *     Ka = [sx 0   0 ]
     *          [0  sy  0 ]
     *          [0  0   sz]
     * </pre>
     * and
     * <pre>
     *     Ta = [1          -alphaXy    alphaXz ]
     *          [alphaYx    1           -alphaYz]
     *          [-alphaZx   alphaZy     1       ]
     * </pre>
     * Hence:
     * <pre>
     *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
     *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
     *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
     * </pre>
     * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
     * are considered to be zero if the accelerometer z-axis is assumed to be the same
     * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
     * becomes upper diagonal:
     * <pre>
     *     Ma = [sx    mxy  mxz]
     *          [0     sy   myz]
     *          [0     0    sz ]
     * </pre>
     * Values of this matrix are unit-less.
     *
     * @return estimated accelerometer scale factors and cross coupling errors, or null
     * if not available.
     */
    @Override
    public Matrix getEstimatedMa() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 409
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1093
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4127
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5535
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
    }

    /**
     * Makes proper conversion of internal cross-coupling, bias and g-dependent
     * cross bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @param g internal g-dependent cross bias matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m, final Matrix g) throws AlgebraException {
        setResult(m);

        // Gg = M*G
        m.multiply(g, estimatedGg);
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal scaling and cross-coupling matrix.
     * @throws AlgebraException if a numerical instability occurs.
     */
    private void setResult(final Matrix m) throws AlgebraException {
        // Because:
        // M = I + Mg
        // b = M^-1*bg

        // Then:
        // Mg = M - I
        // bg = M*b

        if (estimatedMg == null) {
            estimatedMg = m;
        } else {
            estimatedMg.copyFrom(m);
        }

        for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
            estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
        }

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5216
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8514
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6253
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7386
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5735
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8973
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6752
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7863
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4574
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5610
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5109
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6126
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 332
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 331
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1050
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1049
setupWmmEstimator();

            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5789
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5895
final var initialB = invInitialM.multiplyAndReturnNew(initialBa);

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are measured specific force coordinates
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS];

                // biases b
                for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                    initial[i] = initialB.getElementAtIndex(i);
                }

                // cross coupling errors M
                final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                for (int i = 0, j = BodyKinematics.COMPONENTS; i < num; i++, j++) {
                    initial[j] = initialM.getElementAtIndex(i);
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 703
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 711
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2651
final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/generators/AccelerometerAndGyroscopeMeasurementsGenerator.java 738
com/irurueta/navigation/inertial/calibration/generators/AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator.java 829
gyroscopeMeasurementsGenerator.reset();

        if (listener != null) {
            listener.onReset(this);
        }
    }

    /**
     * Gets estimated average angular rate during initialization phase.
     *
     * @return estimated average angular rate during initialization phase.
     */
    public AngularSpeedTriad getInitialAvgAngularSpeedTriad() {
        return gyroscopeMeasurementsGenerator.getInitialAvgAngularSpeedTriad();
    }

    /**
     * Gets estimated average angular rate during initialization phase.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialAvgAngularSpeedTriad(final AngularSpeedTriad result) {
        gyroscopeMeasurementsGenerator.getInitialAvgAngularSpeedTriad(result);
    }

    /**
     * Gets estimated standard deviation of angular rate during initialization phase.
     *
     * @return estimated standard deviation of angular rate during initialization phase.
     */
    public AngularSpeedTriad getInitialAngularSpeedTriadStandardDeviation() {
        return gyroscopeMeasurementsGenerator.getInitialAngularSpeedTriadStandardDeviation();
    }

    /**
     * Gets estimated standard deviation of angular rate during initialization phase.
     *
     * @param result instance where result will be stored.
     */
    public void getInitialAngularSpeedTriadStandardDeviation(final AngularSpeedTriad result) {
        gyroscopeMeasurementsGenerator.getInitialAngularSpeedTriadStandardDeviation(result);
    }

    /**
     * Gets gyroscope base noise level that has been detected during
     * initialization expressed in radians per second (rad/s).
     * This is equal to the standard deviation of the gyroscope measurements
     * during initialization phase.
     *
     * @return gyroscope base noise level.
     */
    public double getGyroscopeBaseNoiseLevel() {
        return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevel();
    }

    /**
     * Gets gyroscope base noise level that has been detected during
     * initialization.
     * This is equal to the standard deviation of the gyroscope measurements
     * during initialization phase.
     *
     * @return gyroscope base noise level.
     */
    public AngularSpeed getGyroscopeBaseNoiseLevelAsMeasurement() {
        return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelAsMeasurement();
    }

    /**
     * Gets gyroscope base noise level that has been detected during
     * initialization.
     * This is equal to the standard deviation of the gyroscope measurements
     * during initialization phase.
     *
     * @param result instance where result will be stored.
     */
    public void getGyroscopeBaseNoiseLevelAsMeasurement(final AngularSpeed result) {
        gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelAsMeasurement(result);
    }

    /**
     * Gets gyroscope base noise level PSD (Power Spectral Density)
     * expressed in (rad^2/s).
     *
     * @return gyroscope base noise level PSD.
     */
    public double getGyroscopeBaseNoiseLevelPsd() {
        return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelPsd();
    }

    /**
     * Gets gyroscope base noise level root PSD (Power Spectral Density)
     * expressed in (rad * s^-0.5)
     *
     * @return gyroscope base noise level root PSD.
     */
    @Override
    public double getGyroscopeBaseNoiseLevelRootPsd() {
        return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelRootPsd();
    }
}
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4282
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4395
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4688
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4784
final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3792
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3905
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias             known accelerometer bias. This
     *                                      must have length 3 and is
     *                                      expressed in meters per squared
     *                                      second (m/s^2).
     * @param accelerometerMa               known accelerometer scale factors
     *                                      and cross coupling matrix. Must
     *                                      be 3x3.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4204
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4302
final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4939
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6111
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity and
     *                                      unknown different orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross
     *                                      biases will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events
     *                                      raised by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5074
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5976
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param bias                          known gyroscope bias. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors
     *                                      matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5370
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7551
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6407
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8680
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10465
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10964
final var angularRateMeasZ2 = biasZ + m1.getElementAtIndex(2);

            final var sqrNormMeas1 = angularRateMeasX1 * angularRateMeasX1 + angularRateMeasY1 * angularRateMeasY1
                    + angularRateMeasZ1 * angularRateMeasZ1;
            final var sqrNormMeas2 = angularRateMeasX2 * angularRateMeasX2 + angularRateMeasY2 * angularRateMeasY2
                    + angularRateMeasZ2 * angularRateMeasZ2;

            final var normMeas1 = Math.sqrt(sqrNormMeas1);
            final var normMeas2 = Math.sqrt(sqrNormMeas2);

            return Math.abs(normMeas1 - normMeas2);

        } catch (final WrongSizeException | InvalidRotationMatrixException
                       | InvalidSourceAndDestinationFrameTypeException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {

        final var meas = new ArrayList<StandardDeviationBodyKinematics>();

        for (final var samplesIndex : samplesIndices) {
            meas.add(this.measurements.get(samplesIndex));
        }

        try {
            final var result = new PreliminaryResult();
            result.estimatedMg = getInitialMg();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5463
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6613
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must be 3x1 and is expressed in
     *                                      radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific
     *                                      forces sensed by the
     *                                      accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by
     *                                      this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5596
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6480
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position                      position where body kinematics
     *                                      measures have been taken.
     * @param turntableRotationRate         constant rotation rate at which
     *                                      the turntable is spinning. Must
     *                                      be expressed in radians per
     *                                      second (rad/s).
     * @param timeInterval                  time interval between measurements
     *                                      being captured expressed in
     *                                      seconds (s).
     * @param measurements                  collection of body kinematics
     *                                      measurements with standard
     *                                      deviations taken at the same
     *                                      position with zero velocity
     *                                      and unknown different
     *                                      orientations.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be
     *                                      used to find a solution. This
     *                                      must have length 3 and is
     *                                      expressed in radians per second
     *                                      (rad/s).
     * @param initialMg                     initial gyroscope scale factors
     *                                      and cross coupling errors matrix.
     *                                      Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent
     *                                      cross biases introduced on the
     *                                      gyroscope by the specific forces
     *                                      sensed by the accelerometer.
     *                                      Must be 3x3.
     * @param listener                      listener to handle events raised
     *                                      by this calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5887
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8026
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6904
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9136
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java 220
com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java 197
if (windowSize < MIN_WINDOW_SIZE) {
            throw new IllegalArgumentException();
        }

        this.windowSize = windowSize;
        reset();
    }

    /**
     * Gets time interval between body kinematics samples expressed in
     * seconds (s).
     *
     * @return time interval between accelerometer triad samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between body kinematics samples expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between body kinematics samples.
     *
     * @return time interval between accelerometer triad samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between body kinematics samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between body kinematics samples.
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
                TimeUnit.SECOND));
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public WindowedBodyKinematicsNoiseEstimatorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4471
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5507
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4472
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4575
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5508
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5611
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5004
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6021
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5005
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5110
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6022
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6127
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised
     *                              by this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13635
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 6392
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 22417
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Converts provided distance instance into its corresponding value expressed in
     * meters.
     *
     * @param distance distance instance to be converted.
     * @return converted value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance into its corresponding value expressed in
     * meters per second.
     *
     * @param speed speed instance to be converted.
     * @return converted value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }

    /**
     * Converts provided acceleration instance into its corresponding value expressed
     * in meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value expressed in meters per squared second.
     */
    private static double convertAccelerationToDouble(final Acceleration acceleration) {
        return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
                AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts provided angular speed into its corresponding value expressed in
     * radians per second.
     *
     * @param angularSpeed angular speed instance to be converted.
     * @return converted value expressed in radians per second.
     */
    private static double convertAngularSpeedToDouble(final AngularSpeed angularSpeed) {
        return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
                AngularSpeedUnit.RADIANS_PER_SECOND);
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6361
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6573
fmeas.setElementAtIndex(0, fmeasX);
            fmeas.setElementAtIndex(1, fmeasY);
            fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
            b.setElementAtIndex(1, by);
            b.setElementAtIndex(2, bz);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3714
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4460
initial[3] = initialSx;
                initial[4] = initialSy;
                initial[5] = initialSz;

                initial[6] = initialMxy;
                initial[7] = initialMxz;
                initial[8] = initialMyz;

                return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myz

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myz) = 0.0

                final var bx = params[0];
                final var by = params[1];
                final var bz = params[2];

                final var sx = params[3];
                final var sy = params[4];
                final var sz = params[5];

                final var mxy = params[6];
                final var mxz = params[7];
                final var myz = params[8];

                final var ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 885
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 256
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 893
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 779
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1712
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 261
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1745
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 258
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 973
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 971
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 975
@Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4710
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4005
jacobian.setElementAt(11, 8, 1.9);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Sets input data into Levenberg-Marquardt fitter.
     *
     * @throws AlgebraException if there are numerical instabilities.
     */
    private void setInputData() throws AlgebraException {

        final var ba = getAccelerometerBiasAsMatrix();
        final var ma = getAccelerometerMa();

        final var measuredBeforeF = new double[BodyKinematics.COMPONENTS];
        final var fixedBeforeF = new double[BodyKinematics.COMPONENTS];

        final var measuredAfterF = new double[BodyKinematics.COMPONENTS];
        final var fixedAfterF = new double[BodyKinematics.COMPONENTS];

        final var numSequences = sequences.size();
        final var x = new Matrix(numSequences, 2 * BodyKinematics.COMPONENTS);
        final var y = new double[numSequences];
        final var standardDeviations = new double[numSequences];

        // make a copy of input sequences that will be used to update
        // kinematics measurements with fixed values for memory efficiency

        fixedSequences = new ArrayList<>();
        for (final var sequence : sequences) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4506
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4589
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg, listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a
     *                      solution. This must have length 3 and is expressed
     *                      in radians per second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4833
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4932
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4019
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4105
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a
     *                      solution. This must have length 3 and is expressed
     *                      in radians per second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4353
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4452
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores                 quality scores corresponding to each provided
     *                                      sequence. The larger the score value the better
     *                                      the quality of the sequence.
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5370
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8680
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6407
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7551
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5887
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9136
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6904
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8026
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6045
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4275
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5639
return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                // biases b
                for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                    initial[i] = initialB.getElementAtIndex(i);
                }

                // upper diagonal cross coupling errors M
                var k = BodyKinematics.COMPONENTS;
                for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 552
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 552
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1075
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1074
setupWmmEstimator();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2157
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2156
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 356
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 355
initialize();

            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROSAC;
File Line
com/irurueta/navigation/inertial/estimators/NEDPositionEstimator.java 2011
com/irurueta/navigation/inertial/estimators/NEDVelocityEstimator.java 2308
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 22396
}

    /**
     * Converts provided time instance to seconds (s).
     *
     * @param time time instance to be converted.
     * @return provided time value expressed in seconds.
     */
    private static double convertTimeToDouble(final Time time) {
        return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
    }

    /**
     * Converts provided angle instance to radians (rad).
     *
     * @param angle angle instance to be converted.
     * @return provided angle value expressed in radians.
     */
    private static double convertAngleToDouble(final Angle angle) {
        return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
    }

    /**
     * Converts provided distance instance to meters (m).
     *
     * @param distance distance instance to be converted.
     * @return provided distance value expressed in meters.
     */
    private static double convertDistanceToDouble(final Distance distance) {
        return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
    }

    /**
     * Converts provided speed instance to meters per second (m/s).
     *
     * @param speed speed instance to be converted.
     * @return provided speed value expressed in meters per second.
     */
    private static double convertSpeedToDouble(final Speed speed) {
        return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5002
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2977
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2865
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2995
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3923
return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                initial[0] = initialSx;
                initial[1] = initialSy;
                initial[2] = initialSz;

                initial[3] = initialMxy;
                initial[4] = initialMxz;
                initial[5] = initialMyz;

                return initial;
            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters sx, sy, sz, mxy, mxz, myz are:

                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = 0.0
                // d(fmeasy)/d(sz) = ftruez
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = 0.0

                final var sx = params[0];
                final var sy = params[1];
                final var sz = params[2];

                final var mxy = params[3];
                final var mxz = params[4];
                final var myz = params[5];

                final var ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4685
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5847
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4810
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5721
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5222
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6358
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5341
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6239
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 571
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 263
this(measurements, biasX, biasY, biasZ, commonAxisUsed);
        this.listener = listener;
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<FrameBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        //noinspection unchecked
        this.measurements = (Collection<FrameBodyKinematics>) measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public AccelerometerCalibratorMeasurementType getMeasurementType() {
        return AccelerometerCalibratorMeasurementType.FRAME_BODY_KINEMATICS;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    @Override
    public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 580
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 275
this(measurements, biasX, biasY, biasZ, commonAxisUsed);
        this.listener = listener;
    }

    /**
     * Gets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<FrameBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body kinematics measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body kinematics measurements taken at different
     *                     frames (positions, orientations and velocities).
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        //noinspection unchecked
        this.measurements = (Collection<FrameBodyKinematics>) measurements;
    }

    /**
     * Indicates the type of measurement or sequence used by this calibrator.
     *
     * @return type of measurement or sequence used by this calibrator.
     */
    @Override
    public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
        return GyroscopeCalibratorMeasurementOrSequenceType.FRAME_BODY_KINEMATICS_MEASUREMENT;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements or sequences
     * in a list or not.
     *
     * @return true if measurements or sequences must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsOrSequencesRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement/sequence or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mg matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mg matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    @Override
    public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 596
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1672
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 580
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1640
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 285
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 284
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1001
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1002
setupWmmEstimator();

            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }
}
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5111
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6059
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                return initial;
            }

            @Override
            public double evaluate(final int i, final double[] point, final double[] params,
                                   final double[] derivatives) throws EvaluationException {

                fmeasX = point[0];
                fmeasY = point[1];
                fmeasZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateCommonAxis(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1003
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1029
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
        this(measurements, commonAxisUsed, listener);
        internalSetBias(bias);
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1371
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1502
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3962
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 909
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1050
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4666
var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var omegaMeasX = measuredKinematics.getAngularRateX();
            final var omegaMeasY = measuredKinematics.getAngularRateY();
            final var omegaMeasZ = measuredKinematics.getAngularRateZ();

            final var omegaTrueX = expectedKinematics.getAngularRateX();
            final var omegaTrueY = expectedKinematics.getAngularRateY();
            final var omegaTrueZ = expectedKinematics.getAngularRateZ();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5016
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5653
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {

                measAngularRateX = point[0];
                measAngularRateY = point[1];
                measAngularRateZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateCommonAxis(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4524
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5663
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4627
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5560
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4746
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6879
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5782
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8007
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5058
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6180
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5163
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6075
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have length
     *                              3 and is expressed in radians per
     *                              second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param accelerometerBias     known accelerometer bias. This must
     *                              have length 3 and is expressed in
     *                              meters per squared second
     *                              (m/s^2).
     * @param accelerometerMa       known accelerometer scale factors and
     *                              cross coupling matrix. Must be 3x3.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7366
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6297
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8474
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2490
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3196
for (var j = 0; j < BodyMagneticFluxDensity.COMPONENTS; j++) {
                    for (var i = 0; i < BodyMagneticFluxDensity.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {

                bmeasX = point[0];
                bmeasY = point[1];
                bmeasZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateCommonAxis(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 468
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 496
final RobustKnownFrameAccelerometerCalibratorListener listener) {
        this(measurements, commonAxisUsed);
        this.listener = listener;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2) and only taken into
     * account if non-linear preliminary solutions are used.
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is only taken into account if non-linear preliminary solutions are used.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5929
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2959
measAngularRateZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateGeneral(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];
        final var m21 = result[4];
        final var m31 = result[5];

        final var m12 = result[6];
        final var m22 = result[7];
        final var m32 = result[8];

        final var m13 = result[9];
        final var m23 = result[10];
        final var m33 = result[11];

        final var mb = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5300
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6336
private double evaluate(final double m11, final double m21, final double m31,
                            final double m12, final double m22, final double m32,
                            final double m13, final double m23, final double m33) throws EvaluationException {

        // fmeas = M*(ftrue + b)

        // ftrue = M^-1*fmeas - b

        try {
            if (fmeas == null) {
                fmeas = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (m == null) {
                m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (invM == null) {
                invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (b == null) {
                b = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (ftrue == null) {
                ftrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1676
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1576
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1695
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1594
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
File Line
com/irurueta/navigation/inertial/calibration/bias/BodyKinematicsBiasEstimator.java 1246
com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java 1873
rebuildExpectedKinematics();
    }

    /**
     * Gets ECEF frame containing current body position and orientation expressed
     * in ECEF coordinates. Frame also contains body velocity, but it is always
     * assumed to be zero during calibration.
     *
     * @return ECEF frame containing current body position and orientation resolved
     * around ECEF axes.
     */
    public ECEFFrame getEcefFrame() {
        return new ECEFFrame(frame);
    }

    /**
     * Gets ECEF frame containing current body position and orientation expressed
     * in ECEF coordinates. Frame also contains body velocity, but it is always
     * assumed to be zero during calibration.
     *
     * @param result instance where ECEF frame containing current body position and
     *               orientation resolved around ECEF axes will be stored.
     */
    public void getEcefFrame(final ECEFFrame result) {
        frame.copyTo(result);
    }

    /**
     * Gets NED frame containing current body position and orientation expressed
     * in NED coordinates. Frame also contains body velocity, but it is always
     * assumed to be zero during calibration.
     *
     * @return NED frame containing current body position and orientation resolved
     * around NED axes.
     */
    public NEDFrame getNedFrame() {
        return ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame);
    }

    /**
     * Gets NED frame containing current body position and orientation expressed
     * in NED coordinates. Frame also contains body velocity, but it is always
     * assumed to be zero during calibration.
     *
     * @param result instance where NED frame containing current body position and
     *               orientation resolved around NED axes will be stored.
     */
    public void getNedFrame(final NEDFrame result) {
        ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
    }

    /**
     * Gets current body position expressed in NED coordinates.
     *
     * @return current body position expressed in NED coordinates.
     */
    public NEDPosition getNedPosition() {
        return getNedFrame().getPosition();
    }

    /**
     * Gets current body position expressed in NED coordinates.
     *
     * @param result instance where current body position will be stored.
     */
    public void getNedPosition(final NEDPosition result) {
        getNedFrame().getPosition(result);
    }

    /**
     * Sets current body position expressed in NED coordinates.
     *
     * @param position current body position to be set.
     * @throws LockedException if estimator is currently running.
     */
    public void setNedPosition(final NEDPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        final var nedFrame = getNedFrame();
        nedFrame.setPosition(position);
        NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5119
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4379
b = new Matrix(BodyKinematics.COMPONENTS, 1);
            }
            if (g == null) {
                g = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }
            if (tmp == null) {
                tmp = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3546
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3914
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4206
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4618
estimatedMg.setElementAt(1, 1, sy);

        estimatedMg.setElementAt(0, 2, mxz);
        estimatedMg.setElementAt(1, 2, myz);
        estimatedMg.setElementAt(2, 2, sz);

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedGg.setElementAtIndex(0, g11);
        estimatedGg.setElementAtIndex(1, g21);
        estimatedGg.setElementAtIndex(2, g31);
        estimatedGg.setElementAtIndex(3, g12);
        estimatedGg.setElementAtIndex(4, g22);
        estimatedGg.setElementAtIndex(5, g32);
        estimatedGg.setElementAtIndex(6, g13);
        estimatedGg.setElementAtIndex(7, g23);
        estimatedGg.setElementAtIndex(8, g33);

        estimatedCovariance = fitter.getCovar();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4746
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8007
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5141
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7306
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5782
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6879
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6178
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8434
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8474
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5659
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7782
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6297
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7366
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6676
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8892
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2685
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3473
final double m11, final double m21, final double m31,
            final double m12, final double m22, final double m32,
            final double m13, final double m23, final double m33) throws EvaluationException {

        // bmeas = M*(btrue + b)

        // btrue = M^-1*bmeas - b

        try {
            if (bmeas == null) {
                bmeas = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            }
            if (m == null) {
                m = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }
            if (invM == null) {
                invM = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }
            if (b == null) {
                b = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            }
            if (btrue == null) {
                btrue = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
            }
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1955
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }


    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1219
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3463
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1189
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3398
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1215
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3458
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1185
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3395
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4521
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4856
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1911
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1993
}

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public L getListener() {
        return listener;
    }

    /**
     * Sets listener to handle events raised by this estimator.
     *
     * @param listener listener to handle events raised by this estimator.
     * @throws LockedException if calibrator is currently running.
     */
    public void setListener(final L listener) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 82
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 78
UnknownBiasGyroscopeCalibrator, GyroscopeCalibrationSource, GyroscopeBiasUncertaintySource,
        OrderedBodyKinematicsSequenceGyroscopeCalibrator, QualityScoredGyroscopeCalibrator,
        AccelerometerDependentGyroscopeCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = true;

    /**
     * Indicates that by default G-dependent cross biases introduced
     * by the accelerometer on the gyroscope are estimated.
     */
    public static final boolean DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES = true;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzy;

    /**
     * Initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double initialBiasX;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3916
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4000
final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5252
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3034
final var mg = preliminaryResult.estimatedMg;

        final var gg = preliminaryResult.estimatedGg;

        try {
            final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            m1.add(mg);

            final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final var m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3426
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3510
final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must be 3x1 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must be 3x1
     *                          and is expressed in meters per squared
     *                          second (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4871
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7015
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5004
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7159
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5141
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8434
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5908
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8143
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6041
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8287
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6178
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7306
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6572
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7817
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6690
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7700
final double[] bias, final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 7 samples.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5399
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7497
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5525
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7638
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5659
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8892
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6416
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8605
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6542
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8746
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6676
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7782
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7067
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8289
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7181
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8173
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
                    timeInterval, measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores         quality scores corresponding to each provided
     *                              measurement. The larger the score value the better
     *                              the quality of the sample.
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size, if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 samples.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
File Line
com/irurueta/navigation/inertial/calibration/generators/AccelerometerAndGyroscopeMeasurementsGenerator.java 253
com/irurueta/navigation/inertial/calibration/generators/AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator.java 331
gyroscopeMeasurementsGenerator.setTimeInterval(timeInterval);
    }

    /**
     * Gets time interval between input samples.
     *
     * @return time interval between input samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(getTimeInterval(), TimeUnit.SECOND);
    }

    /**
     * Gets time interval between input samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(getTimeInterval());
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between input samples.
     *
     * @param timeInterval time interval between input samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
                TimeUnit.SECOND));
    }

    /**
     * Gets minimum number of samples required in a static interval to be taken into account.
     * Smaller static intervals will be discarded.
     *
     * @return minimum number of samples required in a static interval to be taken into account.
     */
    public int getMinStaticSamples() {
        return accelerometerMeasurementsGenerator.getMinStaticSamples();
    }

    /**
     * Sets minimum number of samples required in a static interval to be taken into account.
     * Smaller static intervals will be discarded.
     *
     * @param minStaticSamples minimum number of samples required in a static interval to be
     *                         taken into account.
     * @throws LockedException          if generator is busy.
     * @throws IllegalArgumentException if provided value is less than 2.
     */
    public void setMinStaticSamples(final int minStaticSamples) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        accelerometerMeasurementsGenerator.setMinStaticSamples(minStaticSamples);
        gyroscopeMeasurementsGenerator.setMinStaticSamples(minStaticSamples);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 596
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1641
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 580
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1673
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured angular rates and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates gyroscope calibration parameters containing scale factors,
     * cross-coupling errors and G-dependent coupling.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5002
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2977
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 480
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4157
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5823
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5929
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2959
fmeasZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateGeneral(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];
        final var m21 = result[4];
        final var m31 = result[5];

        final var m12 = result[6];
        final var m22 = result[7];
        final var m32 = result[8];

        final var m13 = result[9];
        final var m23 = result[10];
        final var m33 = result[11];

        final var bias = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1081
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2688
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2910
}

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 468
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2865
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3225
} catch (final AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 1024
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1101
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1250
b.setElementAtIndex(i, fMeasZ - fTrueZ);
            i++;
        }

        final var unknowns = Utils.solve(a, b);

        final var bx = unknowns.getElementAtIndex(0);
        final var by = unknowns.getElementAtIndex(1);
        final var bz = unknowns.getElementAtIndex(2);
        final var sx = unknowns.getElementAtIndex(3);
        final var sy = unknowns.getElementAtIndex(4);
        final var sz = unknowns.getElementAtIndex(5);
        final var mxy = unknowns.getElementAtIndex(6);
        final var mxz = unknowns.getElementAtIndex(7);
        final var myx = unknowns.getElementAtIndex(8);
        final var myz = unknowns.getElementAtIndex(9);
        final var mzx = unknowns.getElementAtIndex(10);
        final var mzy = unknowns.getElementAtIndex(11);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4302
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4941
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4145
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4747
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  list of body kinematics measurements taken at a given position with
     *                      different unknown orientations and containing the standard deviations
     *                      of accelerometer and gyroscope measurements.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4828
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5465
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final Matrix initialMa,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(qualityScores,
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(qualityScores,
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4659
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5248
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  list of body kinematics measurements taken at a given position with
     *                      different unknown orientations and containing the standard deviations
     *                      of accelerometer and gyroscope measurements.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 13 samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1085
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3158
}

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3025
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 413
}

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurementsOrSequences() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the calibration.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors,
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3604
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4267
for (int j = 6, i = 9; j < 15; j++, i++) {
            jacobian.setElementAt(i, j, 1.0);
        }

        // propagated covariance is J * Cov * J'
        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {

        // The gyroscope model is:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Hence:
        // [Ωmeasx] = [bx] + ( [1   0   0] + [sx    mxy    mxz]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [0   1   0]   [myx   sy     myz]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [0   0   1]   [mzx   mzy    sz ]  [Ωtruez]   [g31   g32   g33][ftruez]

        // [Ωmeasx] = [bx] + ( [1+sx  mxy    mxz ]) [Ωtruex] + [g11   g12   g13][ftruex]
        // [Ωmeasy]   [by]     [myx   1+sy   myz ]  [Ωtruey]   [g21   g22   g23][ftruey]
        // [Ωmeasz]   [bz]     [mzx   mzy    1+sz]  [Ωtruez]   [g31   g32   g33][ftruez]

        // Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
        // g31, g32, g33
        // Reordering:
        // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
        // Ωmeasy - Ωtruey - by = myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
        // Ωmeasz - Ωtruez - bz = mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

        // [Ωtruex  0       0       Ωtruey  Ωtruez  0       0       0       0       ftruex  ftruey  ftruez  0       0       0       0       0       0     ][sx ] =  [Ωmeasx - Ωtruex - bx]
        // [0       Ωtruey  0       0       0       Ωtruex  Ωtruez  0       0       0       0       0       ftruex  ftruey  ftruez  0       0       0     ][sy ]    [Ωmeasy - Ωtruey - by]
        // [0       0       Ωtruez  0       0       0       0       Ωtruex  Ωtruey  0       0       0       0       0       0       ftruex  ftruey  ftruez][sz ]    [Ωmeasz - Ωtruez - bz]
        //                                                                                                                                                 [mxy]
        //                                                                                                                                                 [mxz]
        //                                                                                                                                                 [myx]
        //                                                                                                                                                 [myz]
        //                                                                                                                                                 [mzx]
        //                                                                                                                                                 [mzy]
        //                                                                                                                                                 [g11]
        //                                                                                                                                                 [g12]
        //                                                                                                                                                 [g13]
        //                                                                                                                                                 [g21]
        //                                                                                                                                                 [g22]
        //                                                                                                                                                 [g23]
        //                                                                                                                                                 [g31]
        //                                                                                                                                                 [g32]
        //                                                                                                                                                 [g33]

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are true angular rate + specific force coordinates
                return 2 * BodyKinematics.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured angular rate
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS];

                initial[0] = initialSx;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3760
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3829
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences   collection of sequences containing timestamped body
     *                    kinematics measurements.
     * @param initialBias initial gyroscope bias to be used to find a
     *                    solution. This must have length 3 and is expressed
     *                    in radians per second (rad/s).
     * @param initialMg   initial gyroscope scale factors and cross coupling
     *                    errors matrix. Must be 3x3.
     * @param initialGg   initial gyroscope G-dependent cross biases
     *                    introduced on the gyroscope by the specific forces
     *                    sensed by the accelerometer. Must be 3x3.
     * @param method      robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4043
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4130
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3553
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3640
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences                     collection of sequences containing timestamped body
     *                                      kinematics measurements.
     * @param commonAxisUsed                indicates whether z-axis is
     *                                      assumed to be common for
     *                                      accelerometer and gyroscope.
     * @param estimateGDependentCrossBiases true if G-dependent cross biases
     *                                      will be estimated, false
     *                                      otherwise.
     * @param initialBias                   initial gyroscope bias to be used to find a
     *                                      solution. This must be 3x1 and is expressed
     *                                      in radians per second (rad/s).
     * @param initialMg                     initial gyroscope scale factors and cross coupling
     *                                      errors matrix. Must be 3x3.
     * @param initialGg                     initial gyroscope G-dependent cross biases
     *                                      introduced on the gyroscope by the specific forces
     *                                      sensed by the accelerometer. Must be 3x3.
     * @param listener                      listener to handle events raised by this
     *                                      calibrator.
     * @param method                        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4871
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8143
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5004
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8287
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5908
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7015
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6041
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7159
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10441
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10940
final var mg = preliminaryResult.estimatedMg;

            final var gg = preliminaryResult.estimatedGg;

            final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            m1.add(mg);

            final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final var m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5399
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8605
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5525
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8746
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6416
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7497
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6542
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7638
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4842
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5513
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix hardIron, final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 8030
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6995
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setInitialHardIron(preliminaryResult.estimatedHardIron);
                innerCalibrator.setInitialMm(preliminaryResult.estimatedMm);
                innerCalibrator.setCommonAxisUsed(commonAxisUsed);
                innerCalibrator.setGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5330
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5997
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix initialHardIron, final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1345
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 984
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1455
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 876
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();

            a.setElementAt(i, 0, fTrueX);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 794
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 164
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 801
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 576
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 687
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 561
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 170
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 167
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 882
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 879
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 884
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1576
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1285
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1255
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 411
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4280
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5100
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3790
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4620
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 405
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1590
}

    /**
     * Gets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @return a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     */
    @Override
    public Collection<FrameBodyMagneticFluxDensity> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a collection of body magnetic flux density measurements taken at different
     * frames (positions, orientations and velocities).
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate the a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     *
     * @param measurements collection of body magnetic flux density measurements
     *                     taken at different frames (positions, orientations
     *                     and velocities).
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setMeasurements(final Collection<? extends FrameBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        //noinspection unchecked
        this.measurements = (Collection<FrameBodyMagneticFluxDensity>) measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public MagnetometerCalibratorMeasurementType getMeasurementType() {
        return MagnetometerCalibratorMeasurementType.FRAME_BODY_MAGNETIC_FLUX_DENSITY;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return false;
    }

    /**
     * Indicates whether this calibrator requires quality scores for each
     * measurement or not.
     *
     * @return true if quality scores are required, false otherwise.
     */
    @Override
    public boolean isQualityScoresRequired() {
        return false;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    @Override
    public KnownFrameMagnetometerLinearLeastSquaresCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java 225
com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java 182
reset();
    }

    /**
     * Gets time interval between body kinematics samples expressed in
     * seconds (s).
     *
     * @return time interval between accelerometer triad samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between body kinematics samples expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between body kinematics samples.
     *
     * @return time interval between accelerometer triad samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between body kinematics samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between body kinematics samples.
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
                TimeUnit.SECOND));
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public WindowedBodyKinematicsNoiseEstimatorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3978
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4378
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4726
return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final var bx = params[0];
                final var by = params[1];
                final var bz = params[2];

                final var sx = params[3];
                final var sy = params[4];
                final var sz = params[5];

                final var mxy = params[6];
                final var mxz = params[7];
                final var myx = params[8];
                final var myz = params[9];
                final var mzx = params[10];
                final var mzy = params[11];

                final var ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1676
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1594
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1576
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1695
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples
     *                                  (10 or 13).
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4638
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4735
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4154
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4253
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores     quality scores corresponding to each provided
     *                          sequence. The larger the score value the better
     *                          the quality of the sequence.
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1943
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1948
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
        internalSetQualityScores(qualityScores);
    }


    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2039
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 469
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2239
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 497
this(measurements, commonAxisUsed, initialBias, initialMa);
        this.listener = listener;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3131
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3673
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3039
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3549
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position position where body kinematics measures have been taken.
     * @param method   robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3659
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4200
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3564
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4068
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position position where body kinematics measures have been taken.
     * @param method   robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1219
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3399
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1189
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3464
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1215
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3396
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1185
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3459
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sequence.
     * The larger the score value the better the quality of the sequence.
     *
     * @param qualityScores quality scores corresponding to each sequence.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether calibrator is ready to find a solution.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3419
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4004
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @param method   robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3927
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4506
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron, final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position position where body magnetic flux density measurements
     *                 have been taken.
     * @param method   robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9356
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9401
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, convertDistanceToDouble(oldX),
                convertDistanceToDouble(oldY), convertDistanceToDouble(oldZ), oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/calibration/BodyKinematicsGenerator.java 117
com/irurueta/navigation/inertial/calibration/BodyKinematicsGenerator.java 310
final IMUErrors errors, final Random random, final Collection<BodyKinematics> result) {
        try {
            final var trueFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
            final var ma = errors.getAccelerometerScaleFactorAndCrossCouplingErrors();
            final var ba = errors.getAccelerometerBiasesAsMatrix();
            final var trueOmegaIbb = new Matrix(BodyKinematics.COMPONENTS, 1);
            final var mg = errors.getGyroScaleFactorAndCrossCouplingErrors();
            final var bg = errors.getGyroBiasesAsMatrix();
            final var gg = errors.getGyroGDependentBiases();
            final var identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            final var tmp33 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            final var tmp31a = new Matrix(BodyKinematics.COMPONENTS, 1);
            final var tmp31b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1823
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 717
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2657
this.listener = listener;
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return known x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX known x coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return known y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY known y coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return known z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ known z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return known x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1676
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1576
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 340
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1695
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1594
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3399
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 344
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3464
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 346
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1955
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1943
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1948
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3101
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4934
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                // g-dependent cross biases G
                final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                for (int i = 0, j = k; i < num; i++, j++) {
                    initial[j] = initG.getElementAtIndex(i);
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4237
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5642
private double evaluateCommonAxisWithGDependentCrossBiases(final int i, final double[] params)
            throws EvaluationException {

        final var m11 = params[0];

        final var m12 = params[1];
        final var m22 = params[2];

        final var m13 = params[3];
        final var m23 = params[4];
        final var m33 = params[5];

        final var g11 = params[6];
        final var g21 = params[7];
        final var g31 = params[8];

        final var g12 = params[9];
        final var g22 = params[10];
        final var g32 = params[11];

        final var g13 = params[12];
        final var g23 = params[13];
        final var g33 = params[14];

        return evaluate(i, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33, g11, g21, g31, g12, g22, g32,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4683
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6811
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7939
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5220
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7301
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6237
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8409
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java 109
com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java 144
com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java 226
}

    /**
     * Gets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @return time interval between accelerometer triad samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between accelerometer triad samples expressed in
     * seconds (s).
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @return time interval between accelerometer triad samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between accelerometer triad samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between accelerometer triad samples.
     *
     * @param timeInterval time interval between accelerometer triad samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(),
                timeInterval.getUnit(), TimeUnit.SECOND));
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public L getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1791
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1694
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1810
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1712
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1632
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3514
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 459
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3579
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 459
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2060
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2057
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2062
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1808
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1712
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 474
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1828
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1734
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1649
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3534
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 478
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3597
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 477
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2079
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2077
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2081
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3982
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4630
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3840
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4442
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4510
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5157
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4353
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4945
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4393
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5225
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3903
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4745
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4513
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5184
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5001
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5667
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/FrameBodyKinematics.java 352
com/irurueta/navigation/inertial/calibration/FrameBodyMagneticFluxDensity.java 353
}

    /**
     * Gets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around ECEF axes associated to body kinematics measurement.
     *
     * @return current ECEF frame associated to body kinematics measurement or null if
     * not available.
     */
    public ECEFFrame getFrame() {
        return frame;
    }

    /**
     * Sets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around ECEF axes associated to body kinematics measurement.
     *
     * @param frame current ECEF frame
     */
    public void setFrame(final ECEFFrame frame) {
        this.frame = frame;
    }

    /**
     * Gets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around NED axes associated to body kinematics measurement.
     *
     * @return current NED frame associated to body kinematics measurement or null if
     * not available.
     */
    public NEDFrame getNedFrame() {
        return frame != null ? ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame) : null;
    }

    /**
     * Gets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around NED axes associated to body kinematics measurement.
     *
     * @param result instance where result data will be stored if available.
     * @return true if result instance was updated, false otherwise.
     */
    public boolean getNedFrame(final NEDFrame result) {
        if (frame != null) {
            ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets current body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around NED axes associated to body kinematics measurement.
     * <p>
     * This method will internally store the corresponding ECEF frame to provided
     * NED frame value.
     *
     * @param nedFrame current NED frame associated to body kinematics measurement
     *                 to be set.
     */
    public void setNedFrame(final NEDFrame nedFrame) {
        if (nedFrame != null) {
            if (frame != null) {
                NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
            } else {
                frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
            }
        } else {
            frame = null;
        }
    }

    /**
     * Gets previous body position (which will typically remain constant),
     * velocity (which will typically be zero) and orientation (which usually changes
     * with each measurement to perform calibration of a single device) resolved
     * around ECEF axes associated to previous body kinematics measurement.
     *
     * @return previous ECEF frame associated to previous body kinematics measurement or
     * null if not available.
     */
    public ECEFFrame getPreviousFrame() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4355
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1026
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2423
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1314
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1736
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1030
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2970
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2417
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3499
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1333
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3461
}

    /**
     * Gets known bias as a column matrix.
     *
     * @return known bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known bias as an array.
     *
     * @param bias bias to be set.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1089
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 409
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3033
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3166
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is ready to start the estimator.
     *
     * @return true if estimator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether estimator is currently running or not.
     *
     * @return true if estimator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if estimator is currently running.
     * @throws NotReadyException    if estimator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2696
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2918
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1093
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 421
public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or not.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3789
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4075
final var mzy = params[8];

                final var g11 = params[9];
                final var g21 = params[10];
                final var g31 = params[11];
                final var g12 = params[12];
                final var g22 = params[13];
                final var g32 = params[14];
                final var g13 = params[15];
                final var g23 = params[16];
                final var g33 = params[17];

                final var omegatruex = point[0];
                final var omegatruey = point[1];
                final var omegatruez = point[2];

                final var ftruex = point[3];
                final var ftruey = point[4];
                final var ftruez = point[5];

                result[0] = biasX + omegatruex + sx * omegatruex + mxy * omegatruey + mxz * omegatruez
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1614
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1634
public MSACRobustKnownBiasTurntableGyroscopeCalibrator(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener) {
        super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1646
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1666
public MSACRobustTurntableGyroscopeCalibrator(
            final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener) {
        super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4683
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7939
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5072
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7232
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5719
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6811
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6109
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8360
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5220
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8409
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5594
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7710
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6237
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7301
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6611
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8820
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 3978
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2046
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1229
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1338
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1294
this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @return known accelerometer bias.
     */
    @Override
    public AccelerationTriad getBiasAsTriad() {
        return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known accelerometer bias.
     *
     * @param result instance where result will be stored.
     */
    @Override
    public void getBiasAsTriad(final AccelerationTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Sets known accelerometer bias.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBias(final AccelerationTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
        biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
        biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor.
     *
     * @return initial x scaling factor.
     */
    @Override
    public double getInitialSx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5795
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4518
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5901
return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS];

                // biases b
                for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                    initial[i] = initialB.getElementAtIndex(i);
                }

                // cross coupling errors M
                final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                for (int i = 0, j = BodyKinematics.COMPONENTS; i < num; i++, j++) {
                    initial[j] = initialM.getElementAtIndex(i);
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1642
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1664
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 790
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 799
}

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        gravityNorm = computeGravityNorm();

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 704
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 814
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 759
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 185
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 823
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 768
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 708
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1641
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 190
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1673
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 187
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 909
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 900
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 905
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3984
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4139
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4138
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4783
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4218
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4861
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4260
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4900
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4632
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4784
final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3841
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3988
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3987
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4589
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4063
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4665
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4104
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4706
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4443
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4590
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4512
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4667
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4665
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5308
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4745
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5386
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4786
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5424
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5159
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5310
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4355
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4504
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4502
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5091
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4578
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5167
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4618
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5207
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4947
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5093
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3695
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4009
m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);

        final var g = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        g.setElementAtIndex(0, g11);
        g.setElementAtIndex(1, g21);
        g.setElementAtIndex(2, g31);

        g.setElementAtIndex(3, g12);
        g.setElementAtIndex(4, g22);
        g.setElementAtIndex(5, g32);

        g.setElementAtIndex(6, g13);
        g.setElementAtIndex(7, g23);
        g.setElementAtIndex(8, g33);

        setResult(m, b, g);

        // at this point covariance is expressed in terms of b, M and G, and must
        // be expressed in terms of bg, Mg and Gg.
        // We know that:
        // bg = M * b
        // Mg = M - I
        // Gg = M * G

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11  m12  m13]
        //     [0    m22  m23]
        //     [0    0    m33]

        // G = [g11  g12  g13]
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // bg = [m11  m12  m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
        //      [0    m22  m23][by]   [           m22 * by + m23 * bz]   [bgy]
        //      [0    0    m33][bz]   [                      m33 * bz]   [bgz]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [0          m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [0          0           m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [0    m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [0    0    m33][g31  g32  g33]

        // Defining the linear application:
        // F(b, M, G) = F(bx, by, bz, m11, m12, m22, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
        // as:
        // [bgx] =  [m11 * bx + m12 * by + m13 * bz]
        // [bgy]    [           m22 * by + m23 * bz]
        // [bgz]    [                      m33 * bz]
        // [sx]     [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]
        // [gg11]   [m11 * g11 + m12 * g21 + m13 * g31]
        // [gg21]   [            m22 * g21 + m23 * g31]
        // [gg31]   [                        m33 * g31]
        // [gg12]   [m11 * g12 + m12 * g22 + m13 * g32]
        // [gg22]   [            m22 * g22 + m23 * g32]
        // [gg32]   [                        m33 * g32]
        // [gg13]   [m11 * g13 + m12 * g23 + m13 * g33]
        // [gg23]   [            m22 * g23 + m23 * g33]
        // [gg33]   [                        m33 * g33]

        // Then the Jacobian of F(b, M, G) is:
        // J = [m11  m12  m13  bx   by   0    bz   0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    m22  m23  0    0    by   0    bz   0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    m33  0    0    0    0    0    bz   0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    g11  g21  0    g31  0    0    m11  m12  m13  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    g21  0    g31  0    0    m22  m23  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    g31  0    0    m33  0    0    0    0    0    0  ]
        //     [0    0    0    g12  g22  0    g32  0    0    0    0    0    m11  m12  m13  0    0    0  ]
        //     [0    0    0    0    0    g22  0    g32  0    0    0    0    0    m22  m23  0    0    0  ]
        //     [0    0    0    0    0    0    0    0    g32  0    0    0    0    0    m33  0    0    0  ]
        //     [0    0    0    g13  g23  0    g33  0    0    0    0    0    0    0    0    m11  m12  m13]
        //     [0    0    0    0    0    g23  0    g33  0    0    0    0    0    0    0    0    m22  m23]
        //     [0    0    0    0    0    0    0    0    g33  0    0    0    0    0    0    0    0    m33]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5882
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 11015
innerCalibrator.setSequences(seqs);
            innerCalibrator.calibrate();

            innerCalibrator.getEstimatedBiases(result.estimatedBiases);
            result.estimatedMg = innerCalibrator.getEstimatedMg();
            result.estimatedGg = innerCalibrator.getEstimatedGg();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5014
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5359
mm.setElementAtIndex(5, 0.0);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mb, mg);

        // at this point covariance is expressed in terms of b, M and G, and must
        // be expressed in terms of bg, Mg and Gg.
        // We know that:
        // bg = M * b
        // Mg = M - I
        // Gg = M * G

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11  m12  m13]
        //     [0    m22  m23]
        //     [0    0    m33]

        // G = [g11  g12  g13]
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // bg = [m11  m12  m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
        //      [0    m22  m23][by]   [           m22 * by + m23 * bz]   [bgy]
        //      [0    0    m33][bz]   [                      m33 * bz]   [bgz]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [0          m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [0          0           m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [0    m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [0    0    m33][g31  g32  g33]

        // Defining the linear application:
        // F(b, M, G) = F(bx, by, bz, m11, m12, m22, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
        // as:
        // [bgx] =  [m11 * bx + m12 * by + m13 * bz]
        // [bgy]    [           m22 * by + m23 * bz]
        // [bgz]    [                      m33 * bz]
        // [sx]     [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]
        // [gg11]   [m11 * g11 + m12 * g21 + m13 * g31]
        // [gg21]   [            m22 * g21 + m23 * g31]
        // [gg31]   [                        m33 * g31]
        // [gg12]   [m11 * g12 + m12 * g22 + m13 * g32]
        // [gg22]   [            m22 * g22 + m23 * g32]
        // [gg32]   [                        m33 * g32]
        // [gg13]   [m11 * g13 + m12 * g23 + m13 * g33]
        // [gg23]   [            m22 * g23 + m23 * g33]
        // [gg33]   [                        m33 * g33]

        // Then the Jacobian of F(b, M, G) is:
        // J = [m11  m12  m13  bx   by   0    bz   0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    m22  m23  0    0    by   0    bz   0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    m33  0    0    0    0    0    bz   0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    1    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    g11  g21  0    g31  0    0    m11  m12  m13  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    g21  0    g31  0    0    m22  m23  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    g31  0    0    m33  0    0    0    0    0    0  ]
        //     [0    0    0    g12  g22  0    g32  0    0    0    0    0    m11  m12  m13  0    0    0  ]
        //     [0    0    0    0    0    g22  0    g32  0    0    0    0    0    m22  m23  0    0    0  ]
        //     [0    0    0    0    0    0    0    0    g32  0    0    0    0    0    m33  0    0    0  ]
        //     [0    0    0    g13  g23  0    g33  0    0    0    0    0    0    0    0    m11  m12  m13]
        //     [0    0    0    0    0    g23  0    g33  0    0    0    0    0    0    0    0    m22  m23]
        //     [0    0    0    0    0    0    0    0    g33  0    0    0    0    0    0    0    0    m33]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5138
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5301
final double[] hardIron,
            final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron,
                    listener);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron,
                    listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param hardIron                           known hard-iron.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron matrix is not 3x1,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4515
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4672
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4670
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5341
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param initialMm     initial soft-iron matrix containing scale factors
     *                      and cross coupling errors.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4754
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5425
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final Matrix initialMm, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, initialMm, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4798
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5469
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix hardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5186
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5343
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5651
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5814
final double[] initialHardIron,
            final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron,
                    listener);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron,
                    listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param initialHardIron                    initial hard-iron to find a solution.
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron matrix is not 3x1,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5003
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5160
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5158
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5825
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5242
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5909
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final Matrix initialMm, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, initialMm, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5286
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5953
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix initialHardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5669
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5827
final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6361
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5797
fmeas.setElementAtIndex(0, fmeasX);
            fmeas.setElementAtIndex(1, fmeasY);
            fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1005
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1117
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1073
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1819
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2900
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1031
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1823
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2864
internalSetBias(bias);
    }

    /**
     * Gets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets known x coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasX x coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return y coordinate of accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets known y coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasY y coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasY = biasY;
    }

    /**
     * Gets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @return z coordinate of accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets known z coordinate of accelerometer bias expressed in meters per squared
     * second (m/s^2).
     *
     * @param biasZ z coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.biasZ = biasZ;
    }

    /**
     * Gets known x coordinate of accelerometer bias.
     *
     * @return x coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3755
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4021
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param biasX         known x coordinate of accelerometer bias.
     * @param biasY         known y coordinate of accelerometer bias.
     * @param biasZ         known z coordinate of accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1412
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1382
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1430
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1401
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 766
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 752
innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3812
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4074
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param biasX         known x coordinate of gyroscope bias.
     * @param biasY         known y coordinate of gyroscope bias.
     * @param biasZ         known z coordinate of gyroscope bias.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4472
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5611
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4575
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5508
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param bias                  known gyroscope bias. This must have length
     *                              3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustKnownBiasTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4808
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6947
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4937
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7087
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5072
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8360
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5845
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8075
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5974
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8215
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6109
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7232
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5005
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6127
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must be 3x1 and
     *                              is expressed in radians per second
     *                              (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by this
     *                              calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5110
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6022
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param position              position where body kinematics measures
     *                              have been taken.
     * @param turntableRotationRate constant rotation rate at which the
     *                              turntable is spinning. Must be
     *                              expressed in radians per second (rad/s).
     * @param timeInterval          time interval between measurements being
     *                              captured expressed in seconds (s).
     * @param measurements          collection of body kinematics
     *                              measurements with standard deviations
     *                              taken at the same position with zero
     *                              velocity and unknown different
     *                              orientations.
     * @param initialBias           initial gyroscope bias to be used to
     *                              find a solution. This must have
     *                              length 3 and is expressed in radians
     *                              per second (rad/s).
     * @param initialMg             initial gyroscope scale factors and
     *                              cross coupling errors matrix. Must
     *                              be 3x3.
     * @param initialGg             initial gyroscope G-dependent cross
     *                              biases introduced on the gyroscope by
     *                              the specific forces sensed by the
     *                              accelerometer. Must be 3x3.
     * @param listener              listener to handle events raised by
     *                              this calibrator.
     * @param method                robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if either
     *                                  turntable rotation rate or
     *                                  time interval is zero or negative.
     */
    public static RobustTurntableGyroscopeCalibrator create(
            final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5339
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7432
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5461
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7567
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5594
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8820
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6356
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8540
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6478
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8675
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6611
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7710
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 285
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 284
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1001
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1002
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 537
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 535
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2146
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2142
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 555
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 555
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2160
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2159
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 359
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 358
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1078
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1077
setupWmmEstimator();

            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 8065
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 7031
innerCalibrator.setGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
                innerCalibrator.setMeasurements(inlierMeasurements);
                innerCalibrator.calibrate();

                estimatedHardIron = innerCalibrator.getEstimatedHardIron();
                estimatedMm = innerCalibrator.getEstimatedMm();

                if (keepCovariance) {
                    estimatedCovariance = innerCalibrator.getEstimatedCovariance();
                } else {
                    estimatedCovariance = null;
                }

                estimatedMse = innerCalibrator.getEstimatedMse();
                estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            } catch (final LockedException | CalibrationException | NotReadyException e) {
                estimatedCovariance = preliminaryResult.covariance;
                estimatedHardIron = preliminaryResult.estimatedHardIron;
                estimatedMm = preliminaryResult.estimatedMm;
                estimatedMse = preliminaryResult.estimatedMse;
                estimatedChiSq = preliminaryResult.estimatedChiSq;
            }
        } else {
            estimatedCovariance = preliminaryResult.covariance;
            estimatedHardIron = preliminaryResult.estimatedHardIron;
            estimatedMm = preliminaryResult.estimatedMm;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }

    /**
     * Internally sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made, expressed in Teslas (T).
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     */
    private void internalSetGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 3758
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 709
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 1824
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1006
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1118
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1074
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 718
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2658
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 1820
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 2901
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1032
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 1824
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 2865
}

    /**
     * Gets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasX() {
        return biasX;
    }

    /**
     * Sets x-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasX x-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasX(final double biasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasX = biasX;
    }

    /**
     * Gets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return y-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasY() {
        return biasY;
    }

    /**
     * Sets y-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasY y-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasY(final double biasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasY = biasY;
    }

    /**
     * Gets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return z-coordinate of known accelerometer bias.
     */
    @Override
    public double getBiasZ() {
        return biasZ;
    }

    /**
     * Sets z-coordinate of known accelerometer bias.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param biasZ z-coordinate of known accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setBiasZ(final double biasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.biasZ = biasZ;
    }

    /**
     * Gets x-coordinate of known accelerometer bias.
     *
     * @return x-coordinate of known accelerometer bias.
     */
    @Override
    public Acceleration getBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4086
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1138
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1094
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2241
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 499
}

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasX() {
        return initialBiasX;
    }

    /**
     * Sets initial x-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasX initial x-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasX(final double initialBiasX) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasX = initialBiasX;
    }

    /**
     * Gets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial y-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasY() {
        return initialBiasY;
    }

    /**
     * Sets initial y-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasY initial y-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasY(final double initialBiasY) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasY = initialBiasY;
    }

    /**
     * Gets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @return initial z-coordinate of accelerometer bias.
     */
    @Override
    public double getInitialBiasZ() {
        return initialBiasZ;
    }

    /**
     * Sets initial z-coordinate of accelerometer bias to be used to find a solution.
     * This is expressed in meters per squared second (m/s^2).
     *
     * @param initialBiasZ initial z-coordinate of accelerometer bias.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.initialBiasZ = initialBiasZ;
    }

    /**
     * Gets initial x-coordinate of accelerometer bias to be used to find a solution.
     *
     * @return initial x-coordinate of accelerometer bias.
     */
    @Override
    public Acceleration getInitialBiasXAsAcceleration() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6363
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6675
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3500
fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
            b.setElementAtIndex(1, by);
            b.setElementAtIndex(2, bz);

            invM.multiply(fmeas, ftrue);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6409
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6132
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
                innerCalibrator.setMeasurements(inlierMeasurements);
                innerCalibrator.calibrate();

                estimatedBiases = innerCalibrator.getEstimatedBiases();
                estimatedMa = innerCalibrator.getEstimatedMa();
                estimatedMse = innerCalibrator.getEstimatedMse();
                estimatedChiSq = innerCalibrator.getEstimatedChiSq();

                if (keepCovariance) {
                    estimatedCovariance = innerCalibrator.getEstimatedCovariance();
                } else {
                    estimatedCovariance = null;
                }
            } catch (final LockedException | CalibrationException | NotReadyException e) {
                estimatedCovariance = preliminaryResult.covariance;
                estimatedBiases = preliminaryResult.estimatedBiases;
                estimatedMa = preliminaryResult.estimatedMa;
                estimatedMse = preliminaryResult.estimatedMse;
                estimatedChiSq = preliminaryResult.estimatedChiSq;
            }
        } else {
            estimatedCovariance = preliminaryResult.covariance;
            estimatedBiases = preliminaryResult.estimatedBiases;
            estimatedMa = preliminaryResult.estimatedMa;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3169
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3440
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4468
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4767
mm.setElementAtIndex(5, 0.0);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mg);

        // at this point covariance is expressed in terms of M and G, and must
        // be expressed in terms of Mg and Gg.
        // We know that:
        // Mg = M - I
        // Gg = M * G

        // M = [m11  m12  m13]
        //     [0    m22  m23]
        //     [0    0    m33]

        // G = [g11  g12  g13]
        //     [g21  g22  g23]
        //     [g31  g32  g33]

        // Mg = [sx  mxy  mxz] = [m11 - 1    m12         m13     ]
        //      [myx sy	  myz]   [0          m22 - 1     m23     ]
        //      [mzx mzy  sz ]   [0          0           m33 - 1 ]

        // Gg = [gg11  gg12  gg13] = [m11  m12  m13][g11  g12  g13]
        //      [gg21  gg22  gg23]   [0    m22  m23][g21  g22  g23]
        //      [gg31  gg32  gg33]   [0    0    m33][g31  g32  g33]

        // Defining the linear application:
        // F(M, G) = F(m11, m12, m22, m32=0, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
        // as:
        // [sx] =   [m11 - 1]
        // [sy]     [m22 - 1]
        // [sz]     [m33 - 1]
        // [mxy]    [m12]
        // [mxz]    [m13]
        // [myx]    [0]
        // [myz]    [m23]
        // [mzx]    [0]
        // [mzy]    [0]
        // [gg11]   [m11 * g11 + m12 * g21 + m13 * g31]
        // [gg21]   [            m22 * g21 + m23 * g31]
        // [gg31]   [                        m33 * g31]
        // [gg12]   [m11 * g12 + m12 * g22 + m13 * g32]
        // [gg22]   [            m22 * g22 + n23 * g32]
        // [gg32]   [                        m33 * g32]
        // [gg13]   [m11 * g13 + m12 * g23 + m13 * g33]
        // [gg23]   [            m22 * g23 + m23 * g33]
        // [gg33]   [                        m33 * g33]

        // Then the Jacobian of F(M, G) is:
        // J = [1    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    1    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    1    0    0    0    0    0    0    0    0    0  ]
        //     [0    1    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    1    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    1    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [0    0    0    0    0    0    0    0    0    0    0    0    0    0    0  ]
        //     [g11  g21  0    g31  0    0    m11  m12  m13  0    0    0    0    0    0  ]
        //     [0    0    g21  0    g31  0    0    m22  m23  0    0    0    0    0    0  ]
        //     [0    0    0    0    0    g31  0    0    m33  0    0    0    0    0    0  ]
        //     [g12  g22  0    g32  0    0    0    0    0    m11  m12  m13  0    0    0  ]
        //     [0    0    g22  0    g32  0    0    0    0    0    m22  m23  0    0    0  ]
        //     [0    0    0    0    0    g32  0    0    0    0    0    m33  0    0    0  ]
        //     [g13  g23  0    g33  0    0    0    0    0    0    0    0    m11  m12  m13]
        //     [0    0    g23  0    g33  0    0    0    0    0    0    0    0    m22  m23]
        //     [0    0    0    0    0    g33  0    0    0    0    0    0    0    0    m33]

        // We know that the propagated covariance is J * Cov * J', hence:
        final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 693
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 680
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1413
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1383
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1431
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1402
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 767
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 753
innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3015
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10921
previousEcefFrame);

        final var angularRateMeasX1 = measuredKinematics.getAngularRateX();
        final var angularRateMeasY1 = measuredKinematics.getAngularRateY();
        final var angularRateMeasZ1 = measuredKinematics.getAngularRateZ();

        final var angularRateTrueX = expectedKinematics.getAngularRateX();
        final var angularRateTrueY = expectedKinematics.getAngularRateY();
        final var angularRateTrueZ = expectedKinematics.getAngularRateZ();

        final var fTrueX = expectedKinematics.getFx();
        final var fTrueY = expectedKinematics.getFy();
        final var fTrueZ = expectedKinematics.getFz();

        final var b = preliminaryResult.estimatedBiases;
        final var bx = b[0];
        final var by = b[1];
        final var bz = b[2];

        final var mg = preliminaryResult.estimatedMg;

        final var gg = preliminaryResult.estimatedGg;
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanInitializer.java 73
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanInitializer.java 77
result.initialize(0.0);

        for (var i = 0; i < 3; i++) {
            result.setElementAt(i, i, initAttUnc2);
        }
        for (var i = 3; i < 6; i++) {
            result.setElementAt(i, i, initVelUnc2);
        }
        for (var i = 6; i < 9; i++) {
            result.setElementAt(i, i, initPosUnc2);
        }
        for (var i = 9; i < 12; i++) {
            result.setElementAt(i, i, initBaUnc2);
        }
        for (var i = 12; i < 15; i++) {
            result.setElementAt(i, i, initBgUnc2);
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1696
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1865
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1768
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 530
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1885
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1787
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1718
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1883
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1787
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 549
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1903
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1809
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 871
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 983
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 930
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 354
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 992
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 939
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1707
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3589
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 534
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3654
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1724
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3609
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 553
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3672
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 877
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1810
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 359
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1842
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2138
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2135
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2157
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2153
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1074
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1070
innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.PROMEDS;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4875
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4160
if (estimatedMg == null) {
            estimatedMg = m;
        } else {
            estimatedMg.copyFrom(m);
        }

        for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
            estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
        }

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
            throws EvaluationException {

        final var bx = params[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4808
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8075
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4937
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8215
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5845
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6947
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5974
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7087
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5339
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8540
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5461
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8675
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6356
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7432
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6478
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7567
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java 924
com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java 926
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2018
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2046
if (isRunning()) {
            throw new LockedException();
        }

        this.position = position;
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @return position where body magnetic flux density measurements have
     * been taken or null if not available.
     */
    public ECEFPosition getEcefPosition() {
        if (position != null) {
            final var result = new ECEFPosition();
            getEcefPosition(result);
            return result;
        } else {
            return null;
        }
    }

    /**
     * Gets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if ECEF position could be computed, false otherwise.
     */
    public boolean getEcefPosition(final ECEFPosition result) {
        if (position != null) {
            final var velocity = new ECEFVelocity();
            NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
                    position.getLatitude(), position.getLongitude(), position.getHeight(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body magnetic flux density measurements have been
     * taken expressed in ECEF coordinates.
     *
     * @param position position where body magnetic flux density have been
     *                 taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final ECEFPosition position) throws LockedException {
        if (isRunning()) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3811
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4558
jacobian.setElementAt(2, 5, ftruez);
                jacobian.setElementAt(2, 6, 0.0);
                jacobian.setElementAt(2, 7, 0.0);
                jacobian.setElementAt(2, 8, 0.0);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var sx = result[3];
        final var sy = result[4];
        final var sz = result[5];

        final var mxy = result[6];
        final var mxz = result[7];
        final var myz = result[8];

        if (estimatedBiases == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 796
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 911
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 859
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 281
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 918
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 865
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1697
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1866
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1769
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 531
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1886
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1788
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1719
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1884
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1788
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 550
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1904
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1810
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 872
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 984
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 931
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 355
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 993
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 940
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 807
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1738
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 287
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1770
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1708
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3590
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 535
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3655
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1725
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3610
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 554
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3673
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 878
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1811
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 360
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1843
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1005
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 997
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2139
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2136
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2158
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2154
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1075
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1071
innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.MSAC;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1509
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1676
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1576
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 340
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1695
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1594
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1220
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1517
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1190
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 344
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 346
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1944
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1955
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1943
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1948
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1507
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1571
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 336
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1690
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1593
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1216
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1511
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1186
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 340
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 339
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 342
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1941
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1948
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1940
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1944
this(measurements, bias, commonAxisUsed, listener);
        internalSetQualityScores(qualityScores);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Returns quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @return quality scores corresponding to each sample.
     */
    @Override
    public double[] getQualityScores() {
        return qualityScores;
    }

    /**
     * Sets quality scores corresponding to each provided sample.
     * The larger the score value the better the quality of the sample.
     *
     * @param qualityScores quality scores corresponding to each sample.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than minimum required samples.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setQualityScores(final double[] qualityScores) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        internalSetQualityScores(qualityScores);
    }

    /**
     * Indicates whether solver is ready to find a solution.
     *
     * @return true if solver is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3673
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3945
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed,
                    listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3681
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4334
final var g33 = result[17];

        final var b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final var m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, 0.0);
        m.setElementAtIndex(2, 0.0);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, 0.0);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3995
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4575
final var g33 = result[20];

        final var b = new Matrix(BodyKinematics.COMPONENTS, 1);
        b.setElementAtIndex(0, bx);
        b.setElementAtIndex(1, by);
        b.setElementAtIndex(2, bz);

        final var m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        m.setElementAtIndex(0, m11);
        m.setElementAtIndex(1, m21);
        m.setElementAtIndex(2, m31);

        m.setElementAtIndex(3, m12);
        m.setElementAtIndex(4, m22);
        m.setElementAtIndex(5, m32);

        m.setElementAtIndex(6, m13);
        m.setElementAtIndex(7, m23);
        m.setElementAtIndex(8, m33);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4223
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5037
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3730
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4000
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3733
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4557
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5000
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5697
final var g33 = result[17];

        final var mb = new Matrix(BodyKinematics.COMPONENTS, 1);
        mb.setElementAtIndex(0, bx);
        mb.setElementAtIndex(1, by);
        mb.setElementAtIndex(2, bz);

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, 0.0);
        mm.setElementAtIndex(2, 0.0);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, 0.0);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5345
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5957
final var g33 = result[20];

        final var mb = new Matrix(BodyKinematics.COMPONENTS, 1);
        mb.setElementAtIndex(0, bx);
        mb.setElementAtIndex(1, by);
        mb.setElementAtIndex(2, bz);

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mm.setElementAtIndex(0, m11);
        mm.setElementAtIndex(1, m21);
        mm.setElementAtIndex(2, m31);

        mm.setElementAtIndex(3, m12);
        mm.setElementAtIndex(4, m22);
        mm.setElementAtIndex(5, m32);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 818
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 826
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 602
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 712
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 585
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 194
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 191
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 906
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 908
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 406
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 735
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 845
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 216
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 854
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1286
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1581
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1256
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 410
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 628
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 739
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 612
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 221
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 409
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 412
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2011
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 218
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 220
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 933
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 940
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 931
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 936
}

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 64
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 70
public abstract class RobustKnownGravityNormAccelerometerCalibrator implements
        AccelerometerNonLinearCalibrator, UnknownBiasNonLinearAccelerometerCalibrator, AccelerometerCalibrationSource,
        AccelerometerBiasUncertaintySource, OrderedStandardDeviationBodyKinematicsAccelerometerCalibrator,
        QualityScoredAccelerometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements when common z-axis is assumed.
     */
    public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
            BaseGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;

    /**
     * Required minimum number of measurements for the general case.
     */
    public static final int MINIMUM_MEASUREMENTS_GENERAL =
            BaseGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default.
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     */
    protected List<StandardDeviationBodyKinematics> measurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownGravityNormAccelerometerCalibratorListener listener;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3872
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3957
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3382
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3467
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences         collection of sequences containing timestamped body
     *                          kinematics measurements.
     * @param initialBias       initial gyroscope bias to be used to find a
     *                          solution. This must have length 3 and is expressed
     *                          in radians per second (rad/s).
     * @param initialMg         initial gyroscope scale factors and cross coupling
     *                          errors matrix. Must be 3x3.
     * @param initialGg         initial gyroscope G-dependent cross biases
     *                          introduced on the gyroscope by the specific forces
     *                          sensed by the accelerometer. Must be 3x3.
     * @param accelerometerBias known accelerometer bias. This must
     *                          have length 3 and is expressed in
     *                          meters per squared second
     *                          (m/s^2).
     * @param accelerometerMa   known accelerometer scale factors and
     *                          cross coupling matrix. Must be 3x3.
     * @param listener          listener to handle events raised by this
     *                          calibrator.
     * @param method            robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 811
com/irurueta/navigation/inertial/calibration/magnetometer/KnownMagneticFluxDensityNormMagnetometerCalibrator.java 854
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made, expressed in Teslas (T).
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm)
            throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }

        internalSetGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
    }

    /**
     * Sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthMagneticFluxDensityNorm(final MagneticFluxDensity groundTruthMagneticFluxDensityNorm)
            throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        if (groundTruthMagneticFluxDensityNorm != null) {
            internalSetGroundTruthMagneticFluxDensityNorm(MagneticFluxDensityConverter.convert(
                    groundTruthMagneticFluxDensityNorm.getValue().doubleValue(),
                    groundTruthMagneticFluxDensityNorm.getUnit(),
                    MagneticFluxDensityUnit.TESLA));
        } else {
            internalSetGroundTruthMagneticFluxDensityNorm(null);
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return super.isReady() && getGroundTruthMagneticFluxDensityNorm() != null;
    }
}
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9266
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9310
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldSpeedX, oldSpeedY, oldSpeedZ, convertAccelerationToDouble(fx), convertAccelerationToDouble(fy),
                convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9618
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9662
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
                oldVx, oldVy, oldVz,
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9707
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9752
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1552
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1504
}

    /**
     * Indicates whether provided measurements are ready to be
     * used for an update.
     *
     * @param measurements measurements to be checked.
     * @return true if estimator is ready, false otherwise.
     */
    public static boolean isUpdateMeasurementsReady(final Collection<GNSSMeasurement> measurements) {
        return GNSSLeastSquaresPositionAndVelocityEstimator.isValidMeasurements(measurements);
    }

    /**
     * Updates GNSS measurements of this estimator when new satellite measurements
     * are available.
     * Calls to this method will be ignored if interval between provided timestamp
     * and last timestamp when Kalman filter was updated is less than epoch interval.
     *
     * @param measurements GNSS measurements to be updated.
     * @param timestamp    timestamp since epoch time when GNSS measurements were
     *                     updated.
     * @return true if measurements were updated, false otherwise.
     * @throws LockedException   if this estimator is already running.
     * @throws NotReadyException if estimator is not ready for measurements updates.
     * @throws INSGNSSException  if estimation fails due to numerical instabilities.
     */
    public boolean updateMeasurements(final Collection<GNSSMeasurement> measurements, final Time timestamp)
            throws LockedException, NotReadyException, INSGNSSException {
        return updateMeasurements(measurements, TimeConverter.convert(timestamp.getValue().doubleValue(),
                timestamp.getUnit(), TimeUnit.SECOND));
    }

    /**
     * Updates GNSS measurements of this estimator when new satellite measurements
     * are available.
     * Call to this method will be ignored if interval between provided timestamp
     * and last timestamp when Kalman filter was updated is less than epoch interval.
     *
     * @param measurements GNSS measurements to be updated.
     * @param timestamp    timestamp expressed in seconds since epoch time when
     *                     GNSS measurements were updated.
     * @return true if measurements were updated, false otherwise.
     * @throws LockedException   if this estimator is already running.
     * @throws NotReadyException if estimator is not ready for measurements updates.
     * @throws INSGNSSException  if estimation fails due to numerical instabilities.
     */
    public boolean updateMeasurements(final Collection<GNSSMeasurement> measurements, final double timestamp)
            throws LockedException, NotReadyException, INSGNSSException {

        if (running) {
            throw new LockedException();
        }

        if (!isUpdateMeasurementsReady(measurements)) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5328
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5795
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6571
getBiasAsMatrix(ba);

            fmeas.setElementAtIndex(0, fmeasX);
            fmeas.setElementAtIndex(1, fmeasY);
            fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1641
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1663
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1285
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1255
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
    }

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3959
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4551
return evaluateGeneralWithGDependentCrossBiases(i, params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];
        final var m21 = result[4];
        final var m31 = result[5];

        final var m12 = result[6];
        final var m22 = result[7];
        final var m32 = result[8];

        final var m13 = result[9];
        final var m23 = result[10];
        final var m33 = result[11];

        final var g11 = result[12];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2041
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3124
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1251
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2045
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3086
this.biasZ = convertAngularSpeed(biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @return known gyroscope bias.
     */
    public AngularSpeedTriad getBiasAsTriad() {
        return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
    }

    /**
     * Gets known gyroscope bias.
     *
     * @param result instance where result will be stored.
     */
    public void getBiasAsTriad(final AngularSpeedTriad result) {
        result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
    }

    /**
     * Sets known gyroscope bias.
     *
     * @param bias gyroscope bias to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setBias(final AngularSpeedTriad bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
        biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
        biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets initial x scaling factor of gyroscope.
     *
     * @return initial x scaling factor of gyroscope.
     */
    @Override
    public double getInitialSx() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5799
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5895
fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, biasX);
            b.setElementAtIndex(1, biasY);
            b.setElementAtIndex(2, biasZ);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5277
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3059
final var angularRateMeasZ2 = biasZ + m1.getElementAtIndex(2);

            final var diffX = angularRateMeasX2 - angularRateMeasX1;
            final var diffY = angularRateMeasY2 - angularRateMeasY1;
            final var diffZ = angularRateMeasZ2 - angularRateMeasZ1;

            return Math.sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);

        } catch (final WrongSizeException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {

        final var meas = new ArrayList<StandardDeviationFrameBodyKinematics>();

        for (final var samplesIndex : samplesIndices) {
            meas.add(this.measurements.get(samplesIndex));
        }

        try {
            final var result = new PreliminaryResult();
            result.estimatedMg = getInitialMg();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6575
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6675
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3500
fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
            b.setElementAtIndex(1, by);
            b.setElementAtIndex(2, bz);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5330
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6361
fmeas.setElementAtIndex(0, fmeasX);
            fmeas.setElementAtIndex(1, fmeasY);
            fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6365
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5128
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6677
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3502
m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
            b.setElementAtIndex(1, by);
            b.setElementAtIndex(2, bz);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5257
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 10944
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            m1.add(mg);

            final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final var m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10445
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3039
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            m1.add(mg);

            final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            angularRateTrue.setElementAtIndex(0, angularRateTrueX);
            angularRateTrue.setElementAtIndex(1, angularRateTrueY);
            angularRateTrue.setElementAtIndex(2, angularRateTrueZ);

            m1.multiply(angularRateTrue);

            final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
            fTrue.setElementAtIndex(0, fTrueX);
            fTrue.setElementAtIndex(1, fTrueY);
            fTrue.setElementAtIndex(2, fTrueZ);
            final var m2 = gg.multiplyAndReturnNew(fTrue);

            m1.add(m2);

            final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2715
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3498
bmeas.setElementAtIndex(0, bmeasX);
            bmeas.setElementAtIndex(1, bmeasY);
            bmeas.setElementAtIndex(2, bmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3714
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3983
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ, commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ, commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2855
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3405
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2785
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3294
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3386
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3935
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3312
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3816
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4336
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5162
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3771
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4038
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3846
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4682
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
                    accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5402
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10514
innerCalibrator.setSequences(seqs);
            innerCalibrator.calibrate();

            result.estimatedMg = innerCalibrator.getEstimatedMg();
            result.estimatedGg = innerCalibrator.getEstimatedGg();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4632
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4064
jacobian.setElementAt(9, 8, 1.0);
        // propagated covariance is J * Cov * J'
        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     * @throws IOException                              if world magnetic model cannot be loaded.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException, com.irurueta.numerical.NotReadyException,
            IOException {
        // The magnetometer model is:
        // mBmeas = ba + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = ba + (I + Mm) * mBtrue

        // Hence:
        // [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        // [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        // [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        // [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        // [mBmeasy]   [by]     [myx    1+sy    myz ][mBtruey]
        // [mBmeasz]   [bz]     [mzx    mzy     1+sz][mBtruez]

        // mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + myx * mBtruex + (1+sy) * mBtruey + myz * mBtruez
        // mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        // mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
        // mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez

        // mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy - mBtruey = by + myx * mBtruex + sy * mBtruey + myz * mBtruez
        // mBmeasz - mBtruez = bz + mzx * mBtruex + mzy * mBtruey + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0        0        0        0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruex  mBtruez  0        0      ][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0        0        mBtruex  mBtruey][bz ]   [mBmeasz - mBtruez]
        //                                                                                              [sx ]
        //                                                                                              [sy ]
        //                                                                                              [sz ]
        //                                                                                              [mxy]
        //                                                                                              [mxz]
        //                                                                                              [myx]
        //                                                                                              [myz]
        //                                                                                              [mzx]
        //                                                                                              [mzy]

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are true magnetic flux density coordinates
                return BodyMagneticFluxDensity.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured magnetic flux density
                return BodyMagneticFluxDensity.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS];

                initial[0] = initialHardIronX;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3123
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3708
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3631
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4210
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5007
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 473
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2982
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2982
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1693
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 4243
}

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases.
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5827
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3959
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4551
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5933
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2963
return evaluateGeneral(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];
        final var m21 = result[4];
        final var m31 = result[5];

        final var m12 = result[6];
        final var m22 = result[7];
        final var m32 = result[8];

        final var m13 = result[9];
        final var m23 = result[10];
        final var m33 = result[11];

        final var bias = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 707
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 711
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
        super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 60
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 65
public abstract class RobustKnownBiasAndGravityNormAccelerometerCalibrator implements
        AccelerometerNonLinearCalibrator, KnownBiasAccelerometerCalibrator,
        OrderedStandardDeviationBodyKinematicsAccelerometerCalibrator, QualityScoredAccelerometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements when common z-axis is assumed.
     */
    public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
            BaseBiasGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;

    /**
     * Required minimum number of measurements for the general case.
     */
    public static final int MINIMUM_MEASUREMENTS_GENERAL =
            BaseBiasGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default.
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     */
    protected List<StandardDeviationBodyKinematics> measurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1632
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 2290
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 2249
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2870
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 485
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3230
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4162
}

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return array containing x,y,z components of estimated accelerometer biases.
     */
    @Override
    public double[] getEstimatedBiases() {
        return estimatedBiases;
    }

    /**
     * Gets array containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where estimated accelerometer biases will be stored.
     * @return true if result instance was updated, false otherwise (when estimation
     * is not yet available).
     */
    @Override
    public boolean getEstimatedBiases(final double[] result) {
        if (estimatedBiases != null) {
            System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @return column matrix containing x,y,z components of estimated accelerometer
     * biases
     */
    @Override
    public Matrix getEstimatedBiasesAsMatrix() {
        return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
    }

    /**
     * Gets column matrix containing x,y,z components of estimated accelerometer biases
     * expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be stored.
     * @return true if result was updated, false otherwise.
     * @throws WrongSizeException if provided result instance has invalid size.
     */
    @Override
    public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
        if (estimatedBiases != null) {
            result.fromArray(estimatedBiases);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets x coordinate of estimated accelerometer bias expressed in meters per
     * squared second (m/s^2).
     *
     * @return x coordinate of estimated accelerometer bias or null if not available.
     */
    @Override
    public Double getEstimatedBiasFx() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4626
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6747
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5662
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7875
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5162
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7238
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6179
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8346
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 65
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 76
public abstract class RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator implements
        MagnetometerNonLinearCalibrator, KnownHardIronMagnetometerCalibrator,
        OrderedStandardDeviationBodyMagneticFluxDensityMagnetometerCalibrator, QualityScoredMagnetometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for the accelerometer,
     * gyroscope and magnetometer.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements when common z-axis is assumed.
     */
    public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
            BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;

    /**
     * Required minimum number of measurements for the general case.
     */
    public static final int MINIMUM_MEASUREMENTS_GENERAL =
            BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default.
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     */
    protected List<StandardDeviationBodyMagneticFluxDensity> measurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 65
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 76
public abstract class RobustKnownMagneticFluxDensityNormMagnetometerCalibrator implements
        MagnetometerNonLinearCalibrator, UnknownHardIronNonLinearMagnetometerCalibrator,
        OrderedStandardDeviationBodyMagneticFluxDensityMagnetometerCalibrator, QualityScoredMagnetometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for the accelerometer,
     * gyroscope and magnetometer.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements when common z-axis is assumed.
     */
    public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
            BaseMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;

    /**
     * Required minimum number of measurements for the general case.
     */
    public static final int MINIMUM_MEASUREMENTS_GENERAL =
            BaseMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default.
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     */
    protected List<StandardDeviationBodyMagneticFluxDensity> measurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 7985
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6949
final var diff = groundTruthMagneticFluxDensityNorm - norm;

            return diff * diff;

        } catch (final AlgebraException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {

        final var meas = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();

        for (final var samplesIndex : samplesIndices) {
            meas.add(this.measurements.get(samplesIndex));
        }

        try {
            final var result = new PreliminaryResult();
            result.estimatedHardIron = getInitialHardIron();
            result.estimatedMm = getInitialMm();

            innerCalibrator.setInitialHardIron(result.estimatedHardIron);
            innerCalibrator.setInitialMm(result.estimatedMm);
            innerCalibrator.setCommonAxisUsed(commonAxisUsed);
            innerCalibrator.setGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3135
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3885
jacobian.setElementAt(6, 5, 1.0);

        // propagated covariance is J * Cov * J'
        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        // [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        // [fmeasy]   [by]     [myx    1+sy    myz ][ftruey]
        // [fmeasz]   [bz]     [mzx    mzy     1+sz][ftruez]

        // fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        // fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
        // fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0       0       0       0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruex  ftruez  0       0     ][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0       0       ftruex  ftruey][sz ]   [fmeasz - ftruez - bz]
        //                                                                         [mxy]
        //                                                                         [mxz]
        //                                                                         [myx]
        //                                                                         [myz]
        //                                                                         [mzx]
        //                                                                         [mzy]

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Input points are true specific force coordinates
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured specific force
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS];

                initial[0] = initialSx;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 4091
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4840
jacobian.setElementAt(2, 11, ftruey);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var sx = result[3];
        final var sy = result[4];
        final var sz = result[5];

        final var mxy = result[6];
        final var mxz = result[7];
        final var myx = result[8];
        final var myz = result[9];
        final var mzx = result[10];
        final var mzy = result[11];

        if (estimatedBiases == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1577
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 406
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 790
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 799
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1286
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1581
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1256
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 410
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 409
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 412
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2011
}

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1741
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1761
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3467
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3530
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1673
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1705
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2018
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2010
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2014
}

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1642
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1664
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 735
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 845
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 216
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 854
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 628
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 739
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 612
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 221
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 218
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 220
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 933
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 940
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 931
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 936
}

    /**
     * Indicates whether inliers must be computed and kept.
     *
     * @return true if inliers must be computed and kept, false if inliers
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepInliersEnabled() {
        return computeAndKeepInliers;
    }

    /**
     * Specifies whether inliers must be computed and kept.
     *
     * @param computeAndKeepInliers true if inliers must be computed and kept,
     *                              false if inliers only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepInliers = computeAndKeepInliers;
    }

    /**
     * Indicates whether residuals must be computed and kept.
     *
     * @return true if residuals must be computed and kept, false if residuals
     * only need to be computed but not kept.
     */
    public boolean isComputeAndKeepResiduals() {
        return computeAndKeepResiduals;
    }

    /**
     * Specifies whether residuals must be computed and kept.
     *
     * @param computeAndKeepResiduals true if residuals must be computed and kept,
     *                                false if residuals only need to be computed but not kept.
     * @throws LockedException if calibrator is currently running.
     */
    public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.computeAndKeepResiduals = computeAndKeepResiduals;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 61
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 65
AccelerometerNonLinearCalibrator, KnownBiasAccelerometerCalibrator,
        OrderedStandardDeviationFrameBodyKinematicsAccelerometerCalibrator,
        QualityScoredAccelerometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements.
     */
    public static final int MINIMUM_MEASUREMENTS = 4;

    /**
     * Indicates that by default a linear calibrator is used for preliminary solution estimation.
     * The result obtained on each preliminary solution might be later refined.
     */
    public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyKinematics> measurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownBiasAndFrameAccelerometerCalibratorListener listener;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2856
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2989
final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2988
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3534
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3057
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3601
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3094
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3637
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3406
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3535
final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3901
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4059
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4551
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4706
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2786
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2905
final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2904
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3414
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2969
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3479
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3004
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3514
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3295
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3415
final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3761
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3910
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4363
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4512
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3387
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3519
final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3518
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4063
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3586
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4129
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3622
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4164
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3936
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4064
final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4427
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4587
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5077
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5232
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3313
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3432
final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3431
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3935
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3495
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3999
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3529
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4033
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param initialMa      initial scale factors and cross coupling errors matrix.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3817
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3936
final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4273
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4426
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4865
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5015
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3823
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4109
jacobian.setElementAt(0, 7, 0.0);
                jacobian.setElementAt(0, 8, 0.0);
                jacobian.setElementAt(0, 9, ftruex);
                jacobian.setElementAt(0, 10, ftruey);
                jacobian.setElementAt(0, 11, ftruez);
                jacobian.setElementAt(0, 12, 0.0);
                jacobian.setElementAt(0, 13, 0.0);
                jacobian.setElementAt(0, 14, 0.0);
                jacobian.setElementAt(0, 15, 0.0);
                jacobian.setElementAt(0, 16, 0.0);
                jacobian.setElementAt(0, 17, 0.0);

                jacobian.setElementAt(1, 0, 0.0);
                jacobian.setElementAt(1, 1, omegatruey);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3843
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4129
jacobian.setElementAt(1, 8, 0.0);
                jacobian.setElementAt(1, 9, 0.0);
                jacobian.setElementAt(1, 10, 0.0);
                jacobian.setElementAt(1, 11, 0.0);
                jacobian.setElementAt(1, 12, ftruex);
                jacobian.setElementAt(1, 13, ftruey);
                jacobian.setElementAt(1, 14, ftruez);
                jacobian.setElementAt(1, 15, 0.0);
                jacobian.setElementAt(1, 16, 0.0);
                jacobian.setElementAt(1, 17, 0.0);

                jacobian.setElementAt(2, 0, 0.0);
                jacobian.setElementAt(2, 1, 0.0);
                jacobian.setElementAt(2, 2, omegatruez);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 3626
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 3767
final double[] hardIron,
            final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param hardIron                           known hard-iron.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron matrix is not 3x1.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5055
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5218
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param hardIron                           known hard-iron.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3124
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3265
final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3264
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3849
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param initialMm    initial soft-iron matrix containing scale factors
     *                     and cross coupling errors.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3339
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3924
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final Matrix initialMm,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3379
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3964
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param initialMm      initial soft-iron matrix containing scale factors
     *                       and cross coupling errors.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3709
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3850
final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4430
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4589
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5101
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5263
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4137
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4278
final double[] initialHardIron,
            final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param initialHardIron                    initial hard-iron to find a solution.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron matrix is not 3x1.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5568
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5731
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron                    initial hard-iron to find a solution.
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3632
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3773
final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3772
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4351
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3847
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4426
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final Matrix initialMm,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3887
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4466
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron, final Matrix initialMm,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4211
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4352
final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4918
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5077
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5584
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5747
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2142
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2169
final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2142
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1210
final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1210
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1237
final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECI frame containing current position, velocity and
     *                     body-to-ECI frame coordinate transformation.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        x coordinate of previous velocity of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldVy        y coordinate of previous velocity of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldVz        z coordinate of previous velocity of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param result       instance where estimated body kinematics will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
     *                                  are not ECI frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8981
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9022
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final ECEFVelocity oldVelocity, final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 188
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 190
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 192
final RobustKnownFrameAccelerometerCalibratorListener listener) {
        super(measurements, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 842
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 790
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 212
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 849
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 796
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 738
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1669
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 218
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1701
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 215
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 933
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 927
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 932
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1791
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1694
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 456
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1811
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1713
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1633
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3515
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 460
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3580
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 460
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2061
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2058
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2063
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double[] getQualityScores() {
                return qualityScores;
            }

            @Override
            public double getThreshold() {
                return stopThreshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 913
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 860
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 284
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 922
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 869
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 807
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1740
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 289
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1772
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 286
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1001
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 999
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1004
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2742
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2964
final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param biasX  known x coordinate of accelerometer bias.
     * @param biasY  known y coordinate of accelerometer bias.
     * @param biasZ  known z coordinate of accelerometer bias.
     * @param method robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
File Line
com/irurueta/navigation/inertial/calibration/bias/BodyKinematicsBiasEstimator.java 1133
com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java 1755
}

    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @return time interval between body kinematics samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @param timeInterval time interval between body kinematics samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(convertTime(timeInterval));
    }

    /**
     * Gets current body position expressed in ECEF coordinates.
     *
     * @return current body position expressed in ECEF coordinates.
     */
    public ECEFPosition getEcefPosition() {
        return frame.getECEFPosition();
    }

    /**
     * Gets current body position expressed in ECEF coordinates.
     *
     * @param result instance where current body position will be stored.
     */
    public void getEcefPosition(final ECEFPosition result) {
        frame.getECEFPosition(result);
    }

    /**
     * Sets current body position expressed in ECEF coordinates.
     *
     * @param position current body position to be set.
     * @throws LockedException if estimator is currently running.
     */
    public void setEcefPosition(final ECEFPosition position) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        frame.setPosition(position);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4465
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4548
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
                    initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a solution.
     *                      This must be 3x1 and is expressed in radians per
     *                      second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param listener      listener to handle events raised by this
     *                      calibrator.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2807
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3024
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param biasX  known x coordinate of gyroscope bias.
     * @param biasY  known y coordinate of gyroscope bias.
     * @param biasZ  known z coordinate of gyroscope bias.
     * @param method robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3975
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4061
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    qualityScores, sequences, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      sequence. The larger the score value the better
     *                      the quality of the sequence.
     * @param sequences     collection of sequences containing timestamped body
     *                      kinematics measurements.
     * @param initialBias   initial gyroscope bias to be used to find a solution.
     *                      This must be 3x1 and is expressed in radians per
     *                      second (rad/s).
     * @param initialMg     initial gyroscope scale factors and cross coupling
     *                      errors matrix. Must be 3x3.
     * @param initialGg     initial gyroscope G-dependent cross biases
     *                      introduced on the gyroscope by the specific forces
     *                      sensed by the accelerometer. Must be 3x3.
     * @param listener      listener to handle events raised by this
     *                      calibrator.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size or if provided
     *                                  quality scores length is smaller
     *                                  than 10.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final double[] qualityScores,
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4523
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6630
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4626
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7875
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5559
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7758
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5662
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6747
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5057
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7122
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5162
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8346
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6074
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8230
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6179
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7238
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 906
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 910
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 882
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 886
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
     * The threshold refers to the amount of error on norm between measured specific forces and the
     * ones generated with estimated calibration parameters provided for each sample.
     *
     * @param threshold threshold to determine whether samples are inliers or not.
     * @throws IllegalArgumentException if provided value is equal or less than zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing hard-iron
     * bias and soft-iron scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1841
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1906
public double[] fixAndReturnNew(
            final double measuredAngularRateX, final double measuredAngularRateY, final double measuredAngularRateZ,
            final double trueFx, final double trueFy, final double trueFz,
            final double biasX, final double biasY, final double biasZ,
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33) throws AlgebraException {

        final var result = new double[BodyKinematics.COMPONENTS];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 973
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1285
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 4091
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4563
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4840
jacobian.setElementAt(2, 11, ftruey);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var sx = result[3];
        final var sy = result[4];
        final var sz = result[5];

        final var mxy = result[6];
        final var mxz = result[7];
        final var myx = result[8];
        final var myz = result[9];
        final var mzx = result[10];
        final var mzy = result[11];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1266
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 977
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2917
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3900
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4550
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3941
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4590
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3760
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4362
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3800
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4402
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4426
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5076
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4468
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5116
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4272
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4864
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4312
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4904
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5236
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2372
measAngularRateZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateGeneral(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
        final var m21 = result[1];
        final var m31 = result[2];

        final var m12 = result[3];
        final var m22 = result[4];
        final var m32 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 101
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 124
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzy;

    /**
     * Initial x-coordinate of gyroscope bias to be used to find a solution.
     * This is expressed in radians per second (rad/s).
     */
    private double initialBiasX;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 96
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 129
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Known x-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasX;

    /**
     * Known y-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasY;

    /**
     * Known z-coordinate of accelerometer bias to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     * This is expressed in meters per squared second (m/s^2).
     */
    private double accelerometerBiasZ;

    /**
     * Known accelerometer x scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSx;

    /**
     * Known accelerometer y scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSy;

    /**
     * Known accelerometer z scaling factor to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerSz;

    /**
     * Known accelerometer x-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxy;

    /**
     * Know accelerometer x-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMxz;

    /**
     * Known accelerometer y-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyx;

    /**
     * Known accelerometer y-z cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMyz;

    /**
     * Known accelerometer z-x cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzx;

    /**
     * Known accelerometer z-y cross coupling error to be used to fix measured
     * specific force and find cross biases introduced by the accelerometer.
     */
    private double accelerometerMzy;

    /**
     * X-coordinate of gyroscope known bias expressed in radians per second
     * (rad/s).
     */
    private double biasX;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5295
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6332
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7634
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8763
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5448
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6485
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7470
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8599
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5816
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6833
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8110
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9220
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5966
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6983
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7949
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9059
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5672
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3215
measAngularRateZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateCommonAxis(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];

        final var m12 = result[4];
        final var m22 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var mb = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4429
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5100
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4471
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5142
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4917
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5583
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4959
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5625
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1716
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1881
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1785
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 547
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1901
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1807
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 869
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 981
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 928
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 352
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 990
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 937
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1428
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1722
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1399
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3607
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 551
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3670
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 764
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 875
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 750
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1808
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 357
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1840
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2154
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2150
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1071
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1067
inliersData = null;
            innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3169
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4468
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5359
mm.setElementAtIndex(5, 0.0);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mg);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3440
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4767
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5014
mm.setElementAtIndex(5, m32);

        mm.setElementAtIndex(6, m13);
        mm.setElementAtIndex(7, m23);
        mm.setElementAtIndex(8, m33);

        final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        mg.setElementAtIndex(0, g11);
        mg.setElementAtIndex(1, g21);
        mg.setElementAtIndex(2, g31);

        mg.setElementAtIndex(3, g12);
        mg.setElementAtIndex(4, g22);
        mg.setElementAtIndex(5, g32);

        mg.setElementAtIndex(6, g13);
        mg.setElementAtIndex(7, g23);
        mg.setElementAtIndex(8, g33);

        setResult(mm, mg);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3671
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5007
return 2 * BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                // upper diagonal cross coupling errors M
                var k = 0;
                for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5370
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5433
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6108
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6171
private void setInputDataWithGDependentCrossBiases()
            throws AlgebraException, InvalidSourceAndDestinationFrameTypeException {
        // compute reference frame at current position
        final var nedPosition = getNedPosition();
        final var nedC = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var nedFrame = new NEDFrame(nedPosition, nedC);
        final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
        final var refKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
                ecefFrame);

        final var refAngularRateX = refKinematics.getAngularRateX();
        final var refAngularRateY = refKinematics.getAngularRateY();
        final var refAngularRateZ = refKinematics.getAngularRateZ();

        final var w2 = turntableRotationRate * turntableRotationRate;

        final var numMeasurements = measurements.size();
        final var x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4099
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4496
result[2] = bz + omegatruez + sz * omegatruez
                        + g31 * ftruex + g32 * ftruey + g33 * ftruez;

                jacobian.setElementAt(0, 0, 1.0);
                jacobian.setElementAt(0, 1, 0.0);
                jacobian.setElementAt(0, 2, 0.0);
                jacobian.setElementAt(0, 3, omegatruex);
                jacobian.setElementAt(0, 4, 0.0);
                jacobian.setElementAt(0, 5, 0.0);
                jacobian.setElementAt(0, 6, omegatruey);
                jacobian.setElementAt(0, 7, omegatruez);
                jacobian.setElementAt(0, 8, 0.0);
                jacobian.setElementAt(0, 9, ftruex);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 5885
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10515
innerCalibrator.getEstimatedBiases(result.estimatedBiases);
            result.estimatedMg = innerCalibrator.getEstimatedMg();
            result.estimatedGg = innerCalibrator.getEstimatedGg();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 5403
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 11018
innerCalibrator.calibrate();

            result.estimatedMg = innerCalibrator.getEstimatedMg();
            result.estimatedGg = innerCalibrator.getEstimatedGg();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = sequences.size();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4523
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7758
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5559
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6630
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5057
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8230
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6074
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7122
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1281
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1366
result.getBuffer());
    }

    /**
     * Fixes provided measured angular rate values by undoing the errors
     * introduced by the gyroscope model to restore the true angular
     * rate.
     *
     * @param measuredAngularRateX x-coordinate of measured angular rate
     *                             expressed in radians per second (rad/s).
     * @param measuredAngularRateY y-coordinate of measured angular rate
     *                             expressed in radians per second (rad/s).
     * @param measuredAngularRateZ z-coordinate of measured angular rate
     *                             expressed in radians per second (rad/s).
     * @param trueFx               x-coordinate of true (i.e. fixed)
     *                             specific force expressed in meters per
     *                             squared second (m/s^2).
     * @param trueFy               y-coordinate of true (i.e. fixed)
     *                             specific force expressed in meters per
     *                             squared second (m/s^2).
     * @param trueFz               z-coordinate of true (i.e. fixed)
     *                             specific force expressed in meters per
     *                             squared second (m/s^2).
     * @param biasX                x-coordinate of bias expressed in
     *                             meters per squared second (m/s^2).
     * @param biasY                y-coordinate of bias expressed in
     *                             meters per squared second (m/s^2).
     * @param biasZ                z-coordinate of bias expressed in
     *                             meters per squared second (m/s^2).
     * @param sx                   initial x scaling factor.
     * @param sy                   initial y scaling factor.
     * @param sz                   initial z scaling factor.
     * @param mxy                  initial x-y cross coupling error.
     * @param mxz                  initial x-z cross coupling error.
     * @param myx                  initial y-x cross coupling error.
     * @param myz                  initial y-z cross coupling error.
     * @param mzx                  initial z-x cross coupling error.
     * @param mzy                  initial z-y cross coupling error.
     * @param g11                  element 1,1 of g-dependant cross biases.
     * @param g21                  element 2,1 of g-dependant cross biases.
     * @param g31                  element 3,1 of g-dependant cross biases.
     * @param g12                  element 1,2 of g-dependant cross biases.
     * @param g22                  element 2,2 of g-dependant cross biases.
     * @param g32                  element 3,2 of g-dependant cross biases.
     * @param g13                  element 1,3 of g-dependant cross biases.
     * @param g23                  element 2,3 of g-dependant cross biases.
     * @param g33                  element 3,3 of g-dependant cross biases.
     * @param result               instance where restored true angular rate
     *                             will be stored. Must have length 3.
     * @throws AlgebraException         if there are numerical instabilities.
     * @throws IllegalArgumentException if any of the provided parameters
     *                                  does not have proper size.
     */
    public void fix(
            final double measuredAngularRateX, final double measuredAngularRateY, final double measuredAngularRateZ,
            final double trueFx, final double trueFy, final double trueFz,
            final double biasX, final double biasY, final double biasZ,
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33, final double[] result) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1347
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1457
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3375
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 878
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 986
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 4164
var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
            final var fMeasY = measuredKinematics.getFy();
            final var fMeasZ = measuredKinematics.getFz();

            final var fTrueX = expectedKinematics.getFx();
            final var fTrueY = expectedKinematics.getFy();
            final var fTrueZ = expectedKinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2670
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2898
final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
                    commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
                    commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 5349
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5435
this.biasZ = convertAcceleration(biasZ);
    }

    /**
     * Internally sets known accelerometer bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known accelerometer bias.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    private void internalSetBias(final double[] bias) {
        if (bias.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        biasX = bias[0];
        biasY = bias[1];
        biasZ = bias[2];
    }

    /**
     * Internally sets known accelerometer bias as a column matrix.
     * Matrix values are expressed in meters per squared second (m/s^2).
     *
     * @param bias accelerometer bias to be set.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    private void internalSetBias(final Matrix bias) {
        if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
            throw new IllegalArgumentException();
        }

        biasX = bias.getElementAtIndex(0);
        biasY = bias.getElementAtIndex(1);
        biasZ = bias.getElementAtIndex(2);
    }

    /**
     * Converts acceleration value and unit to meters per squared second.
     *
     * @param value acceleration value.
     * @param unit  unit of acceleration value.
     * @return converted value.
     */
    private static double convertAcceleration(final double value, final AccelerationUnit unit) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1930
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1889
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1975
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1936
setGroundTruthGravityNorm(groundTruthGravityNorm != null ? convertAcceleration(groundTruthGravityNorm) : null);
    }

    /**
     * Gets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @return list of body kinematics measurements.
     */
    @Override
    public List<StandardDeviationBodyKinematics> getMeasurements() {
        return measurements;
    }

    /**
     * Sets a list of body kinematics measurements taken at a
     * given position with different unknown orientations and containing the
     * standard deviations of accelerometer and gyroscope measurements.
     *
     * @param measurements list of body kinematics measurements.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public AccelerometerCalibratorMeasurementType getMeasurementType() {
        return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer and gyroscope,
     * false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Ma matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
     *                       and gyroscope, false otherwise.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this estimator.
     *
     * @return listener to handle events raised by this estimator.
     */
    public RobustKnownBiasAndGravityNormAccelerometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3823
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4474
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3943
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4099
final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4058
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4705
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4098
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4744
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4178
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4822
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4591
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4745
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3689
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4288
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3802
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3949
final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3909
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4511
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3948
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4550
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4025
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4627
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4403
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4551
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4347
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4999
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4470
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4627
final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4586
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5231
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4625
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5269
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4705
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5347
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5118
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5271
final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4200
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4792
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4314
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4465
final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4425
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5014
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4463
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5052
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final Matrix initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4540
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5129
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, initialMa);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4906
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5054
final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 570
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 591
public MSACRobustEasyGyroscopeCalibrator(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
        super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                accelerometerBias, accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 555
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 575
public MSACRobustKnownBiasEasyGyroscopeCalibrator(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
            final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
        super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
                accelerometerMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3914
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4686
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2736
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2959
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
                    listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
                    listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3424
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4202
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 597
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1891
}

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Gets array containing x,y,z components of estimated magnetometer
     * hard-iron biases expressed in Teslas (T).
     *
     * @return array containing x,y,z components of estimated magnetometer
     * hard-iron biases.
     */
    @Override
    public double[] getEstimatedHardIron() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2030
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2142
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2058
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 2170
initialSz = initialMm.getElementAtIndex(8);
    }

    /**
     * Gets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @return collection of body magnetic flux density measurements at
     * a known position and timestamp with unknown orientations.
     */
    @Override
    public List<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
        return measurements;
    }

    /**
     * Sets collection of body magnetic flux density measurements taken
     * at a given position with different unknown orientations and containing the
     * standard deviation of magnetometer measurements.
     *
     * @param measurements collection of body magnetic flux density
     *                     measurements at a known position and timestamp
     *                     with unknown orientations.
     * @throws LockedException if calibrator is currently running.
     */
    @Override
    public void setMeasurements(final List<StandardDeviationBodyMagneticFluxDensity> measurements)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.measurements = measurements;
    }

    /**
     * Indicates the type of measurement used by this calibrator.
     *
     * @return type of measurement used by this calibrator.
     */
    @Override
    public MagnetometerCalibratorMeasurementType getMeasurementType() {
        return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
    }

    /**
     * Indicates whether this calibrator requires ordered measurements in a
     * list or not.
     *
     * @return true if measurements must be ordered, false otherwise.
     */
    @Override
    public boolean isOrderedMeasurementsRequired() {
        return true;
    }

    /**
     * Indicates whether z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer.
     * When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
     *
     * @return true if z-axis is assumed to be common for accelerometer,
     * gyroscope and magnetometer, false otherwise.
     */
    @Override
    public boolean isCommonAxisUsed() {
        return commonAxisUsed;
    }

    /**
     * Specifies whether z-axis is assumed to be common for accelerometer and
     * gyroscope.
     * When enabled, this eliminates 3 variables from Mm matrix.
     *
     * @param commonAxisUsed true if z-axis is assumed to be common for
     *                       accelerometer, gyroscope and magnetometer, false
     *                       otherwise.
     * @throws LockedException if estimator is currently running.
     */
    @Override
    public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.commonAxisUsed = commonAxisUsed;
    }

    /**
     * Gets listener to handle events raised by this calibrator.
     *
     * @return listener to handle events raised by this calibrator.
     */
    public RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener getListener() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 4320
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 4470
final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5097
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5260
final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param hardIron                           known hard-iron.
     * @param listener                           listener to handle events raised by this calibrator.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4349
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5020
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4473
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4631
final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4588
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5262
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4629
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5300
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4712
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5383
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final Matrix initialMm, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, initialMm);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param initialMm     initial soft-iron matrix containing scale factors
     *                      and cross coupling errors.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5144
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5302
final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4832
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4982
final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5610
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5773
final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron                    initial hard-iron to find a solution.
     * @param listener                           listener to handle events raised by this calibrator.
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4837
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5503
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4961
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5119
final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5076
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5746
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5117
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5784
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final Matrix initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5200
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5867
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final Matrix initialMm, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, initialMm);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5627
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5786
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2142
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1237
final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs.
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 2169
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1210
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
        estimateKinematics(timeInterval, frame, oldC,
                SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
                SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
                result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param c            body-to-ECEF frame coordinate transformation matrix.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param vx           velocity of body frame x coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vy           velocity of body frame y coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param vz           velocity of body frame z coordinate with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per
     *                     second (m/s).
     * @param oldVx        previous velocity of body frame x coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVy        previous velocity of body frame y coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param oldVz        previous velocity of body frame z coordinate with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters
     *                     per second (m/s).
     * @param position     body position expressed in meters (m) with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8537
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8583
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8892
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8939
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 5831
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4555
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5313
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5937
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 2967
setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];
        final var m21 = result[4];
        final var m31 = result[5];

        final var m12 = result[6];
        final var m22 = result[7];
        final var m32 = result[8];

        final var m13 = result[9];
        final var m23 = result[10];
        final var m33 = result[11];

        final var bias = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1404
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 930
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1439
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 984
fillMa(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateGeneral() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        // [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        // [fmeasy]   [by]     [myx    1+sy    myz ][ftruey]
        // [fmeasz]   [bz]     [mzx    mzy     1+sz][ftruez]

        // fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
        // Reordering:
        // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
        // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

        // fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
        // fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0       0       0       0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruex  ftruez  0       0     ][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0       0       ftruex  ftruey][sz ]   [fmeasz - ftruez - bz]
        //                                                                         [mxy]
        //                                                                         [mxz]
        //                                                                         [myx]
        //                                                                         [myz]
        //                                                                         [mzx]
        //                                                                         [mzy]

        final var expectedKinematics = new BodyKinematics();

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, GENERAL_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1314
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1333
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2421
}

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @return known accelerometer bias as a column matrix.
     */
    @Override
    public Matrix getBiasAsMatrix() {
        Matrix result;
        try {
            result = new Matrix(BodyKinematics.COMPONENTS, 1);
            getBiasAsMatrix(result);
        } catch (final WrongSizeException ignore) {
            // never happens
            result = null;
        }
        return result;
    }

    /**
     * Gets known accelerometer bias as a column matrix.
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void getBiasAsMatrix(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, biasX);
        result.setElementAtIndex(1, biasY);
        result.setElementAtIndex(2, biasZ);
    }

    /**
     * Sets known accelerometer bias as a column matrix.
     *
     * @param bias accelerometer bias to be set.
     * @throws LockedException          if calibrator is currently running
     * @throws IllegalArgumentException if provided matrix is not 3x1.
     */
    @Override
    public void setBias(final Matrix bias) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3602
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3880
final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3984
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4784
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4139
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4632
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3841
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4590
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3988
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4443
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4512
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5310
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4667
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5159
final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4355
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5093
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4504
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4947
final Matrix initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param initialMa     initial scale factors and cross coupling errors matrix.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1429
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1400
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 765
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 751
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 554
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 554
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2159
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2158
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 358
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 357
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1077
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1076
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3660
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3936
final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final double biasX, final double biasY,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 7504
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6995
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                innerCalibrator.setHardIronCoordinates(hardIronX, hardIronY, hardIronZ);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4515
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5343
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4672
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5186
final Matrix hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param initialMm     initial soft-iron matrix containing scale factors
     *                      and cross coupling errors.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5003
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5827
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5160
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5669
final Matrix initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17695
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17750
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18719
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18774
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19197
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19252
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19690
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19742
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20168
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20220
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20649
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20704
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20753
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20802
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20851
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20900
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21299
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21348
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21493
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21542
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1290
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 818
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1304
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 839
}

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope.
     *
     * @throws AlgebraException if there are numerical errors.
     */
    private void calibrateCommonAxis() throws AlgebraException {
        // The accelerometer model is:
        // fmeas = ba + (I + Ma) * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        // [fmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        // where myx = mzx = mzy = 0

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy] = [by]     [0  1   0]   [0     sy  myz]    [ftruey]
        // [fmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [ftruez]

        // [fmeasx] = [bx] +   [1+sx   mxy     mxz ][ftruex]
        // [fmeasy]   [by]     [0      1+sy    myz ][ftruey]
        // [fmeasz]   [bz]     [0      0       1+sz][ftruez]

        // fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + (1+sy) * ftruey + myz * ftruez
        // fmeasz = bz + (1+sz) * ftruez

        // Where the unknowns are: sx, sy, sz, mxy mxz, myz
        // Reordering:
        // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy = by + ftruey + sy * ftruey + myz * ftruez
        // fmeasz = bz + ftruez + sz * ftruez

        // fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
        // fmeasy - ftruey - by = sy * ftruey + myz * ftruez
        // fmeasz - ftruez - bz = sz * ftruez

        // [ftruex  0       0       ftruey  ftruez  0     ][sx ] = [fmeasx - ftruex - bx]
        // [0       ftruey  0       0       0       ftruez][sy ]   [fmeasy - ftruey - by]
        // [0       0       ftruez  0       0       0     ][sz ]   [fmeasz - ftruez - bz]
        //                                                 [mxy]
        //                                                 [mxz]
        //                                                 [myz]

        final var expectedKinematics = new BodyKinematics();

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
        final var b = new Matrix(rows, 1);
        var i = 0;
        for (final var measurement : measurements) {
            final var measuredKinematics = measurement.getKinematics();
            final var ecefFrame = measurement.getFrame();
            final var previousEcefFrame = measurement.getPreviousFrame();
            final var timeInterval = measurement.getTimeInterval();

            ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);

            final var fMeasX = measuredKinematics.getFx();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3098
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3844
if (estimatedMa == null) {
            estimatedMa = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedMa.initialize(0.0);
        }

        estimatedMa.setElementAt(0, 0, sx);

        estimatedMa.setElementAt(0, 1, mxy);
        estimatedMa.setElementAt(1, 1, sy);

        estimatedMa.setElementAt(0, 2, mxz);
        estimatedMa.setElementAt(1, 2, myz);
        estimatedMa.setElementAt(2, 2, sz);

        estimatedCovariance = fitter.getCovar();

        // propagate covariance matrix so that all parameters are taken into
        // account in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy

        // We define a lineal function mapping original parameters for the common
        // axis case to the general case
        // [sx'] = [1  0  0  0  0  0][sx]
        // [sy']   [0  1  0  0  0  0][sy]
        // [sz']   [0  0  1  0  0  0][sz]
        // [mxy']  [0  0  0  1  0  0][mxy]
        // [mxz']  [0  0  0  0  1  0][mxz]
        // [myx']  [0  0  0  0  0  0][myz]
        // [myz']  [0  0  0  0  0  1]
        // [mzx']  [0  0  0  0  0  0]
        // [mzy']  [0  0  0  0  0  0]

        // As defined in com.irurueta.statistics.MultivariateNormalDist,
        // if we consider the jacobian of the lineal application the matrix shown
        // above, then covariance can be propagated as follows
        final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
        jacobian.setElementAt(5, 5, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1717
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1882
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1786
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 548
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1902
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1808
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 870
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 982
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 929
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 353
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 991
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 938
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1723
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3608
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 552
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3671
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 876
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1809
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 358
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1841
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 554
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 554
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2156
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2159
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2152
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2158
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 358
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 357
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1073
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1077
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1069
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1076
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
            innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5787
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5536
final var diff = groundTruthGravityNorm - norm;

            return diff * diff;

        } catch (final AlgebraException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {

        final var meas = new ArrayList<StandardDeviationBodyKinematics>();

        for (final var samplesIndex : samplesIndices) {
            meas.add(measurements.get(samplesIndex));
        }

        try {
            final var result = new PreliminaryResult();
            result.estimatedMa = getInitialMa();

            innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ);
            innerCalibrator.setInitialMa(result.estimatedMa);
            innerCalibrator.setCommonAxisUsed(commonAxisUsed);
            innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5886
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6433
estimatedMa = preliminaryResult.estimatedMa;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }

    /**
     * Internally sets ground truth gravity norm to be expected at location where
     * measurements have been made, expressed in meters per squared second
     * (m/s^2).
     *
     * @param groundTruthGravityNorm ground truth gravity norm or null if
     *                               undefined.
     * @throws IllegalArgumentException if provided value is negative.
     */
    private void internalSetGroundTruthGravityNorm(final Double groundTruthGravityNorm) {
        if (groundTruthGravityNorm != null && groundTruthGravityNorm < 0.0) {
            throw new IllegalArgumentException();
        }
        this.groundTruthGravityNorm = groundTruthGravityNorm;
    }

    /**
     * Converts acceleration value and unit to meters per squared second.
     *
     * @param value acceleration value.
     * @param unit  unit of acceleration value.
     * @return converted value.
     */
    private static double convertAcceleration(final double value, final AccelerationUnit unit) {
        return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
    }

    /**
     * Converts acceleration instance to meters per squared second.
     *
     * @param acceleration acceleration instance to be converted.
     * @return converted value.
     */
    private static double convertAcceleration(final Acceleration acceleration) {
        return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated accelerometer scale factors and cross coupling errors.
         * This is the product of matrix Ta containing cross coupling errors and Ka
         * containing scaling factors.
         * So tat:
         * <pre>
         *     Ma = [sx    mxy  mxz] = Ta*Ka
         *          [myx   sy   myz]
         *          [mzx   mzy  sz ]
         * </pre>
         * Where:
         * <pre>
         *     Ka = [sx 0   0 ]
         *          [0  sy  0 ]
         *          [0  0   sz]
         * </pre>
         * and
         * <pre>
         *     Ta = [1          -alphaXy    alphaXz ]
         *          [alphaYx    1           -alphaYz]
         *          [-alphaZx   alphaZy     1       ]
         * </pre>
         * Hence:
         * <pre>
         *     Ma = [sx    mxy  mxz] = Ta*Ka =  [sx             -sy * alphaXy   sz * alphaXz ]
         *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
         *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
         * </pre>
         * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
         * are considered to be zero if the accelerometer z-axis is assumed to be the same
         * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
         * becomes upper diagonal:
         * <pre>
         *     Ma = [sx    mxy  mxz]
         *          [0     sy   myz]
         *          [0     0    sz ]
         * </pre>
         * Values of this matrix are unit-less.
         */
        private Matrix estimatedMa;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 3035
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 5343
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3122
}

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationFrameBodyKinematics>();
            for (var i = 0; i < nSamples; i++) {
                if (inliers.get(i)) {
                    // sample is inlier
                    inlierMeasurements.add(measurements.get(i));
                }
            }

            try {
                nonLinearCalibrator.setInitialBias(preliminaryResult.estimatedBiases);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3862
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4148
jacobian.setElementAt(2, 8, omegatruey);
                jacobian.setElementAt(2, 9, 0.0);
                jacobian.setElementAt(2, 10, 0.0);
                jacobian.setElementAt(2, 11, 0.0);
                jacobian.setElementAt(2, 12, 0.0);
                jacobian.setElementAt(2, 13, 0.0);
                jacobian.setElementAt(2, 14, 0.0);
                jacobian.setElementAt(2, 15, ftruex);
                jacobian.setElementAt(2, 16, ftruey);
                jacobian.setElementAt(2, 17, ftruez);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var sx = result[0];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4928
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5556
jacobian.setElementAt(17, 17, m33);

        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Internal method to perform calibration when common z-axis is assumed for both
     * the accelerometer and gyroscope and G-dependent cross biases are ignored.
     *
     * @throws AlgebraException                              if there are numerical errors.
     * @throws FittingException                              if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException      if fitter is not ready.
     * @throws InvalidSourceAndDestinationFrameTypeException never happens.
     */
    private void calibrateCommonAxis() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException, InvalidSourceAndDestinationFrameTypeException {

        // The gyroscope model is
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue

        // Since G-dependent cross biases are ignored, we can assume that Gg = 0

        // Hence:
        // Ωmeas = bg + (I + Mg) * Ωtrue

        // For convergence purposes of the Levenberg-Marquardt algorithm, the
        // gyroscope model can be better expressed as:
        // Ωmeas = T*K*(Ωtrue + b)
        // Ωmeas = M*(Ωtrue + b)
        // Ωmeas = M*Ωtrue + M*b

        // where:
        // M = I + Mg
        // bg = M*b = (I + Mg)*b --> b = M^-1*bg

        // We know that the norm of the true angular rate when the device is in a pixed
        // and unknown position and orientation is equal to the Earth rotation rate.
        // ||Ωtrue|| = 7.292115E-5 rad/s

        // Hence
        // Ωmeas - M*b = M*Ωtrue

        // M^-1 * (Ωmeas - M*b) = Ωtrue

        // ||Ωtrue||^2 = (M^-1 * (Ωmeas - M*b))^T*(M^-1 * (Ωmeas - M*b))
        // ||Ωtrue||^2 = (Ωmeas - M*b)^T * (M^-1)^T * M^-1 * (Ωmeas - M*b)
        // ||Ωtrue||^2 = (Ωmeas - M*b)^T * ||M^-1||^2 * (Ωmeas - M*b)
        // ||Ωtrue||^2 = ||Ωmeas - M*b||^2 * ||M^-1||^2

        // Where:

        // b = [bx]
        //     [by]
        //     [bz]

        // M = [m11 	m12 	m13]
        //     [0 		m22 	m23]
        //     [0 	 	0 		m33]

        final var gradientEstimator = new GradientEstimator(this::evaluateCommonAxis);

        final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 448
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 437
private final EasyGyroscopeCalibrator innerCalibrator = new EasyGyroscopeCalibrator();

    /**
     * Contains normalized start gravity coordinates.
     * This is reused when computing error residuals.
     */
    private final InhomogeneousPoint3D startPoint = new InhomogeneousPoint3D();

    /**
     * Contains estimated normalized end gravity coordinates.
     * This is reused when computing error residuals.
     */
    private final InhomogeneousPoint3D endPoint = new InhomogeneousPoint3D();

    /**
     * Contains expected normalized end gravity coordinates.
     * This is reused when computing error residuals.
     */
    private final InhomogeneousPoint3D expectedEndPoint = new InhomogeneousPoint3D();

    /**
     * Contains amount of rotation for a given sequence and preliminary
     * solution.
     * This is reused when computing error residuals.
     */
    private final Quaternion q = new Quaternion();

    /**
     * Array containing measured specific force coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] measuredSpecificForce = new double[BodyKinematics.COMPONENTS];

    /**
     * Array containing fixed specific force coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] fixedSpecificForce = new double[BodyKinematics.COMPONENTS];

    /**
     * Array containing measured angular rate coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] measuredAngularRate = new double[BodyKinematics.COMPONENTS];

    /**
     * Array containing fixed angular rate coordinates.
     * This is reused when computing error residuals.
     */
    private final double[] fixedAngularRate = new double[BodyKinematics.COMPONENTS];

    /**
     * An acceleration fixer.
     * This is reused when computing error residuals.
     */
    private final AccelerationFixer accelerationFixer = new AccelerationFixer();

    /**
     * An angular rate fixer.
     * This is reused when computing error residuals.
     */
    private final AngularRateFixer angularRateFixer = new AngularRateFixer();

    /**
     * Constructor.
     */
    protected RobustEasyGyroscopeCalibrator() {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4173
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4981
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3683
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4501
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4591
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4027
if (estimatedMm == null) {
            estimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
        } else {
            estimatedMm.initialize(0.0);
        }

        estimatedMm.setElementAt(0, 0, sx);

        estimatedMm.setElementAt(0, 1, mxy);
        estimatedMm.setElementAt(1, 1, sy);

        estimatedMm.setElementAt(0, 2, mxz);
        estimatedMm.setElementAt(1, 2, myz);
        estimatedMm.setElementAt(2, 2, sz);

        estimatedCovariance = fitter.getCovar();

        // propagate covariance matrix so that all parameters are taken into
        // account in the order: bx, by, bz, sx, sy, sz, mxy, mxz, myx,
        // myz, mzx, mzy.

        // We define a lineal function mapping original parameters for the common
        // axis case to the general case
        // [bx'] = [1  0  0  0  0  0  0  0  0][bx]
        // [by']   [0  1  0  0  0  0  0  0  0][by]
        // [bz']   [0  0  1  0  0  0  0  0  0][bz]
        // [sx']   [0  0  0  1  0  0  0  0  0][sx]
        // [sy']   [0  0  0  0  1  0  0  0  0][sy]
        // [sz']   [0  0  0  0  0  1  0  0  0][sz]
        // [mxy']  [0  0  0  0  0  0  1  0  0][mxy]
        // [mxz']  [0  0  0  0  0  0  0  1  0][mxz]
        // [myx']  [0  0  0  0  0  0  0  0  0][myz]
        // [myz']  [0  0  0  0  0  0  0  0  1]
        // [mzx']  [0  0  0  0  0  0  0  0  0]
        // [mzy']  [0  0  0  0  0  0  0  0  0]

        // As defined in com.irurueta.statistics.MultivariateNormalDist,
        // if we consider the jacobian of the lineal application the matrix shown
        // above, then covariance can be propagated as follows
        final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
        jacobian.setElementAt(8, 8, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 7562
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 8091
estimatedMm = preliminaryResult.estimatedMm;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }

    /**
     * Internally sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made, expressed in Teslas (T).
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     */
    private void internalSetGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm) {
        if (groundTruthMagneticFluxDensityNorm != null && groundTruthMagneticFluxDensityNorm < 0.0) {
            throw new IllegalArgumentException();
        }
        this.groundTruthMagneticFluxDensityNorm = groundTruthMagneticFluxDensityNorm;
    }

    /**
     * Converts magnetic flux density value and unit to Teslas.
     *
     * @param value magnetic flux density value.
     * @param unit  unit of magnetic flux density value.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
        return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Converts magnetic flux density instance to Teslas.
     *
     * @param magneticFluxDensity magnetic flux density instance to be converted.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
        return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
    }

    /**
     * Internal class containing estimated preliminary result.
     */
    protected static class PreliminaryResult {
        /**
         * Estimated magnetometer soft-iron matrix containing scale factors
         * and cross coupling errors.
         * This is the product of matrix Tm containing cross coupling errors and Km
         * containing scaling factors.
         * So tat:
         * <pre>
         *     Mm = [sx    mxy  mxz] = Tm*Km
         *          [myx   sy   myz]
         *          [mzx   mzy  sz ]
         * </pre>
         * Where:
         * <pre>
         *     Km = [sx 0   0 ]
         *          [0  sy  0 ]
         *          [0  0   sz]
         * </pre>
         * and
         * <pre>
         *     Tm = [1          -alphaXy    alphaXz ]
         *          [alphaYx    1           -alphaYz]
         *          [-alphaZx   alphaZy     1       ]
         * </pre>
         * Hence:
         * <pre>
         *     Mm = [sx    mxy  mxz] = Tm*Km =  [sx             -sy * alphaXy   sz * alphaXz ]
         *          [myx   sy   myz]            [sx * alphaYx   sy              -sz * alphaYz]
         *          [mzx   mzy  sz ]            [-sx * alphaZx  sy * alphaZy    sz           ]
         * </pre>
         * This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
         * are considered to be zero if the accelerometer z-axis is assumed to be the same
         * as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
         * becomes upper diagonal:
         * <pre>
         *     Mm = [sx    mxy  mxz]
         *          [0     sy   myz]
         *          [0     0    sz ]
         * </pre>
         * Values of this matrix are unit-less.
         */
        private Matrix estimatedMm;
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1895
com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java 1064
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
        estimateKinematics(
                TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
                frame.getCoordinateTransformation(), oldC, frame.getVx(), frame.getVy(), frame.getVz(),
                oldVx, oldVy, oldVz, frame.getX(), frame.getY(), frame.getZ(), result);
    }

    /**
     * Estimates body kinematics (specific force applied to a body and its angular rates).
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param frame        body ECEF frame containing current position, velocity and
     *                     body-to-ECEF frame coordinate transformation.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation matrix.
     * @param oldVelocity  previous velocity of body frame with respect ECEF frame.
     * @param result       instance where body kinematics estimation will be stored.
     * @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
     *                                  are not ECEF frame valid.
     */
    public static void estimateKinematics(
            final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16986
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17231
final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
        try {
            navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldFrame          previous NED frame containing body position, velocity and
     *                          coordinate transformation matrix.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateNED(
            final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16987
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17383
final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
        try {
            navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldFrame          previous NED frame containing body position, velocity and
     *                          coordinate transformation matrix.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateNED(
            final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4437
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 4771
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2505
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2727
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1701
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1820
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1774
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 1197
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 1865
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1821
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 2537
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2429
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2506
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3587
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 2927
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 2533
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1718
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2507
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3546
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 1212
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3598
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3634
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1825
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 1907
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3445
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3407
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1201
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1165
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1987
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1952
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2015
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 1980
public void getInitialMa(final Matrix result) {
        if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result.setElementAtIndex(0, initialSx);
        result.setElementAtIndex(1, initialMyx);
        result.setElementAtIndex(2, initialMzx);

        result.setElementAtIndex(3, initialMxy);
        result.setElementAtIndex(4, initialSy);
        result.setElementAtIndex(5, initialMzy);

        result.setElementAtIndex(6, initialMxz);
        result.setElementAtIndex(7, initialMyz);
        result.setElementAtIndex(8, initialSz);
    }

    /**
     * Sets initial scale factors and cross coupling errors matrix.
     *
     * @param initialMa initial scale factors and cross coupling errors matrix.
     * @throws IllegalArgumentException if provided matrix is not 3x3.
     * @throws LockedException          if calibrator is currently running.
     */
    @Override
    public void setInitialMa(final Matrix initialMa) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4993
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5236
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2372
fmeasZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateGeneral(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
        final var m21 = result[1];
        final var m31 = result[2];

        final var m12 = result[3];
        final var m22 = result[4];
        final var m32 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var crossCoupling = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6078
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5672
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3215
fmeasZ = point[2];

                gradientEstimator.gradient(params, derivatives);

                return evaluateCommonAxis(params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var bx = result[0];
        final var by = result[1];
        final var bz = result[2];

        final var m11 = result[3];

        final var m12 = result[4];
        final var m22 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var bias = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 708
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 194
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 191
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 3025
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 3112
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 2965
result.estimatedMa = nonLinearCalibrator.getEstimatedMa();

                if (keepCovariance) {
                    result.covariance = nonLinearCalibrator.getEstimatedCovariance();
                } else {
                    result.covariance = null;
                }

                result.estimatedMse = nonLinearCalibrator.getEstimatedMse();
                result.estimatedChiSq = nonLinearCalibrator.getEstimatedChiSq();
            }

            solutions.add(result);
        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationFrameBodyKinematics>();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4160
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6315
if (estimatedMg == null) {
            estimatedMg = m;
        } else {
            estimatedMg.copyFrom(m);
        }

        for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
            estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
        }

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 712
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates gyroscope calibration parameters containing bias, scale factors
     * cross-coupling errors and g-dependant cross biases.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 910
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 908
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 886
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 879
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 884
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates magnetometer calibration parameters containing soft-iron
     * scale factors and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9443
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9482
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final ECEFVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                convertAngularSpeedToDouble(angularRateX),
                convertAngularSpeedToDouble(angularRateY),
                convertAngularSpeedToDouble(angularRateZ), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 63
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 66
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 70
QualityScoredAccelerometerCalibrator {

    /**
     * Indicates whether by default a common z-axis is assumed for both the accelerometer
     * and gyroscope.
     */
    public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;

    /**
     * Required minimum number of measurements.
     */
    public static final int MINIMUM_MEASUREMENTS = 4;

    /**
     * Indicates that by default a linear calibrator is used for preliminary solution estimation.
     * The result obtained on each preliminary solution might be later refined.
     */
    public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyKinematics> measurements;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2706
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2931
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3532
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3815
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
                    listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
                    listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasY         known y coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasZ         known z coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5821
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5571
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6365
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 6088
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 10518
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 11020
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 7495
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 8021
result.estimatedMa = innerCalibrator.getEstimatedMa();

            if (keepCovariance) {
                result.covariance = innerCalibrator.getEstimatedCovariance();
            } else {
                result.covariance = null;
            }

            result.estimatedMse = innerCalibrator.getEstimatedMse();
            result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();

            solutions.add(result);

        } catch (final LockedException | CalibrationException | NotReadyException e) {
            solutions.clear();
        }
    }

    /**
     * Attempts to refine calibration parameters if refinement is requested.
     * This method returns a refined solution or provided input if refinement is not
     * requested or has failed.
     * If refinement is enabled and it is requested to keep covariance, this method
     * will also keep covariance of refined position.
     *
     * @param preliminaryResult a preliminary result.
     */
    protected void attemptRefine(final PreliminaryResult preliminaryResult) {
        if (refineResult && inliersData != null) {
            final var inliers = inliersData.getInliers();
            final var nSamples = measurements.size();

            final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4138
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4541
jacobian.setElementAt(1, 17, 0.0);

                jacobian.setElementAt(2, 0, 0.0);
                jacobian.setElementAt(2, 1, 0.0);
                jacobian.setElementAt(2, 2, 1.0);
                jacobian.setElementAt(2, 3, 0.0);
                jacobian.setElementAt(2, 4, 0.0);
                jacobian.setElementAt(2, 5, omegatruez);
                jacobian.setElementAt(2, 6, 0.0);
                jacobian.setElementAt(2, 7, 0.0);
                jacobian.setElementAt(2, 8, 0.0);
                jacobian.setElementAt(2, 9, 0.0);
                jacobian.setElementAt(2, 10, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3999
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4783
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4086
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4882
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2771
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2991
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    commonAxisUsed);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3584
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3871
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    biasX, biasY, biasZ, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    biasX, biasY, biasZ, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    biasX, biasY, biasZ, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, biasX, biasY, biasZ, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, biasX, biasY, biasZ, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasY         known y coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasZ         known z coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3509
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4301
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3596
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4402
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg,
            final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3920
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3970
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECI(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1885
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1865
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1616
gyroBiasZ = getValueOrZero(state.getGyroBiasZ());
        } else {
            accelBiasX = 0.0;
            accelBiasY = 0.0;
            accelBiasZ = 0.0;
            gyroBiasX = 0.0;
            gyroBiasY = 0.0;
            gyroBiasZ = 0.0;
        }

        final var fx = kinematics.getFx();
        final var fy = kinematics.getFy();
        final var fz = kinematics.getFz();
        final var angularRateX = kinematics.getAngularRateX();
        final var angularRateY = kinematics.getAngularRateY();
        final var angularRateZ = kinematics.getAngularRateZ();

        correctedKinematics.setSpecificForceCoordinates(fx - accelBiasX, fy - accelBiasY, fz - accelBiasZ);
        correctedKinematics.setAngularRateCoordinates(
                angularRateX - gyroBiasX,
                angularRateY - gyroBiasY,
                angularRateZ - gyroBiasZ);
    }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3222
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3708
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4152
return initial;
            }

            @Override
            public void evaluate(
                    final int i, final double[] point, final double[] result,
                    final double[] params, final Matrix jacobian) {
                // We know that:
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters sx, sy, sz,
                // mxy, mxz, myx, myz, mzx and mzy is:

                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myx) = 0.0
                // d(fmeasx)/d(myz) = 0.0
                // d(fmeasx)/d(mzx) = 0.0
                // d(fmeasx)/d(mzy) = 0.0

                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myx) = ftruex
                // d(fmeasy)/d(myz) = ftruez
                // d(fmeasy)/d(mzx) = 0.0
                // d(fmeasy)/d(mzy) = 0.0

                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myx) = 0.0
                // d(fmeasz)/d(myz) = 0.0
                // d(fmeasz)/d(mzx) = ftruex
                // d(fmeasz)/d(mzy) = ftruey

                final var sx = params[0];
                final var sy = params[1];
                final var sz = params[2];

                final var mxy = params[3];
                final var mxz = params[4];
                final var myx = params[5];
                final var myz = params[6];
                final var mzx = params[7];
                final var mzy = params[8];

                final var ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3722
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3993
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4468
return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myz

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myz) = 0.0

                final var bx = params[0];
                final var by = params[1];
                final var bz = params[2];

                final var sx = params[3];
                final var sy = params[4];
                final var sz = params[5];

                final var mxy = params[6];
                final var mxz = params[7];
                final var myz = params[8];

                final var ftruex = point[0];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 708
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 818
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 189
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 826
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 602
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 712
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 585
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 194
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 191
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 193
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 906
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 910
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 908
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 794
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 164
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 801
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 576
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 687
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 561
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 170
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 167
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 886
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5865
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 5615
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
                innerCalibrator.setMeasurements(inlierMeasurements);
                innerCalibrator.calibrate();

                estimatedMa = innerCalibrator.getEstimatedMa();
                estimatedMse = innerCalibrator.getEstimatedMse();
                estimatedChiSq = innerCalibrator.getEstimatedChiSq();

                if (keepCovariance) {
                    estimatedCovariance = innerCalibrator.getEstimatedCovariance();
                } else {
                    estimatedCovariance = null;
                }
            } catch (final LockedException | CalibrationException | NotReadyException e) {
                estimatedCovariance = preliminaryResult.covariance;
                estimatedMa = preliminaryResult.estimatedMa;
                estimatedMse = preliminaryResult.estimatedMse;
                estimatedChiSq = preliminaryResult.estimatedChiSq;
            }
        } else {
            estimatedCovariance = preliminaryResult.covariance;
            estimatedMa = preliminaryResult.estimatedMa;
            estimatedMse = preliminaryResult.estimatedMse;
            estimatedChiSq = preliminaryResult.estimatedChiSq;
        }
    }
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4909
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5001
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
            throws EvaluationException {

        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 955
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 1099
a.setElementAt(i, 17, fTrueZ);

            b.setElementAtIndex(i, omegaMeasZ - omegaTrueZ);
            i++;
        }

        final var unknowns = Utils.solve(a, b);

        final var bx = unknowns.getElementAtIndex(0);
        final var by = unknowns.getElementAtIndex(1);
        final var bz = unknowns.getElementAtIndex(2);
        final var sx = unknowns.getElementAtIndex(3);
        final var sy = unknowns.getElementAtIndex(4);
        final var sz = unknowns.getElementAtIndex(5);
        final var mxy = unknowns.getElementAtIndex(6);
        final var mxz = unknowns.getElementAtIndex(7);
        final var myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 745
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 730
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 695
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 682
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1415
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1385
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1433
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1404
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 769
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 755
innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4574
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6688
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5610
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7816
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5109
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7179
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6126
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8287
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 337
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 336
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1055
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1054
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 289
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 288
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1005
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1006
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 540
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 538
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2149
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2145
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 558
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 558
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2163
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2162
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 362
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 361
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1081
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1080
innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | IOException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7276
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7307
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final ECEFVelocity oldVelocity, final BodyKinematics kinematics,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
                kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
                kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2786
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2921
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3338
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3469
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2720
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2841
final double[] bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3229
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3351
final double[] bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3315
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3451
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3866
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3998
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3245
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3368
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3749
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3872
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4875
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5566
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6315
if (estimatedMg == null) {
            estimatedMg = m;
        } else {
            estimatedMg.copyFrom(m);
        }

        for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
            estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
        }

        if (estimatedGg == null) {
            estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        } else {
            estimatedGg.initialize(0.0);
        }

        estimatedCovariance = fitter.getCovar();
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Computes gravity versor error at the end of a sequence using provided
     * parameters.
     * This method is internally executed during gradient estimation and
     * Levenberg-Marquardt fitting needed for calibration computation.
     *
     * @param i      row position.
     * @param params array containing parameters for the general purpose case
     *               when G-dependent cross biases are taken into account. Must
     *               have length 18.
     * @return error between estimated and measured gravity versor.
     * @throws EvaluationException if there are numerical instabilities.
     */
    private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5006
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6043
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7308
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8436
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5143
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6180
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7161
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8289
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5527
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6544
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7784
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8894
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5661
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6678
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7640
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8748
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 3553
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 3694
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param hardIron                           known hard-iron.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3052
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3193
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3637
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3778
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4064
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4205
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron                    initial hard-iron to find a solution.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3560
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3701
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4139
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4280
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java 1611
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1562
if (lastStateTimestamp != null && timestamp - lastStateTimestamp <= insEstimator.getEpochInterval()) {
            return false;
        }

        try {
            running = true;

            if (listener != null) {
                listener.onUpdateGNSSMeasurementsStart(this);
            }

            this.measurements = new ArrayList<>(measurements);

            lsEstimator.setMeasurements(this.measurements);
            lsEstimator.setPriorPositionAndVelocityFromEstimation(estimation);
            if (estimation != null) {
                lsEstimator.estimate(estimation);
            } else {
                estimation = lsEstimator.estimate();
            }

            if (listener != null) {
                listener.onUpdateGNSSMeasurementsEnd(this);
            }
        } catch (final GNSSException e) {
            throw new INSGNSSException(e);
        } finally {
            running = false;
        }
File Line
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1334
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1419
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1841
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1906
public void fix(
            final double measuredAngularRateX, final double measuredAngularRateY, final double measuredAngularRateZ,
            final double trueFx, final double trueFy, final double trueFz,
            final double biasX, final double biasY, final double biasZ,
            final double sx, final double sy, final double sz,
            final double mxy, final double mxz, final double myx,
            final double myz, final double mzx, final double mzy,
            final double g11, final double g21, final double g31,
            final double g12, final double g22, final double g32,
            final double g13, final double g23, final double g33, final double[] result) throws AlgebraException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5165
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2545
setResult(crossCoupling);

        // taking into account that:
        // Ma = [sx  mxy  mxz] = [m11  m12  m13]
        //      [myx sy   myz]   [m21  m22  m23]
        //      [mzx mzy  sz ]   [m31  m32  m33]

        // propagate covariance so that all parameters are taken into account
        // in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy

        // We define a lineal function mapping original parameters for the
        // common axis case to the general case
        // [sx'] = [1  0  0  0  0  0][sx ]
        // [sy']   [0  0  1  0  0  0][mxy]
        // [sz']   [0  0  0  0  0  1][sy ]
        // [mxy']  [0  1  0  0  0  0][mxz]
        // [mxz']  [0  0  0  1  0  0][myz]
        // [myx']  [0  0  0  0  0  0][sz ]
        // [myz']  [0  0  0  0  1  0]
        // [mzx']  [0  0  0  0  0  0]
        // [mzy']  [0  0  0  0  0  0]

        // As defined in com.irurueta.statistics.MultivariateNormalDist,
        // if we consider the jacobian of the lineal application the matrix shown
        // above, then covariance can be propagated as follows
        final var jacobian = new Matrix(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
        jacobian.setElementAt(0, 0, 1.0);
        jacobian.setElementAt(1, 2, 1.0);
        jacobian.setElementAt(2, 5, 1.0);
        jacobian.setElementAt(3, 1, 1.0);
        jacobian.setElementAt(4, 3, 1.0);
        jacobian.setElementAt(6, 4, 1.0);
        // propagated covariance is J * Cov * J'
        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
    }

    /**
     * Makes proper conversion of internal cross-coupling and bias matrices.
     *
     * @param m internal cross-coupling matrix.
     */
    private void setResult(final Matrix m) {
        // Because:
        // M = I + Ma

        // Then:
        // Ma = M - I
        if (estimatedMa == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6266
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5001
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6438
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3403
private double evaluateGeneral(final double[] params) throws EvaluationException {
        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];

        return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1510
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1550
b.setElementAtIndex(i, fMeasZ - fTrueZ - biasZ);
            i++;
        }

        final var unknowns = Utils.solve(a, b);

        final var sx = unknowns.getElementAtIndex(0);
        final var sy = unknowns.getElementAtIndex(1);
        final var sz = unknowns.getElementAtIndex(2);
        final var mxy = unknowns.getElementAtIndex(3);
        final var mxz = unknowns.getElementAtIndex(4);
        final var myx = unknowns.getElementAtIndex(5);
        final var myz = unknowns.getElementAtIndex(6);
        final var mzx = unknowns.getElementAtIndex(7);
        final var mzy = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 2689
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 2973
final double biasZ, final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx, final double initialMyz,
            final double initialMzx, final double initialMzy,
            final KnownBiasAndPositionAccelerometerCalibrationListener listener) {
        this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy, listener);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param biasX          x-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param biasY          y-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param biasZ          z-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param initialSx      initial x scaling factor.
     * @param initialSy      initial y scaling factor.
     * @param initialSz      initial z scaling factor.
     * @param initialMxy     initial x-y cross coupling error.
     * @param initialMxz     initial x-z cross coupling error.
     * @param initialMyx     initial y-x cross coupling error.
     * @param initialMyz     initial y-z cross coupling error.
     * @param initialMzx     initial z-x cross coupling error.
     * @param initialMzy     initial z-y cross coupling error.
     */
    public KnownBiasAndPositionAccelerometerCalibrator(
            final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 911
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 1022
a.setElementAt(i, 5, fTrueZ);

            b.setElementAtIndex(i, fMeasZ - fTrueZ);
            i++;
        }

        final var unknowns = Utils.solve(a, b);

        final var bx = unknowns.getElementAtIndex(0);
        final var by = unknowns.getElementAtIndex(1);
        final var bz = unknowns.getElementAtIndex(2);
        final var sx = unknowns.getElementAtIndex(3);
        final var sy = unknowns.getElementAtIndex(4);
        final var sz = unknowns.getElementAtIndex(5);
        final var mxy = unknowns.getElementAtIndex(6);
        final var mxz = unknowns.getElementAtIndex(7);
        final var myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3950
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4346
return BodyKinematics.COMPONENTS;
            }

            @Override
            public int getNumberOfVariables() {
                // The multivariate function returns the components of measured specific force
                return BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS];

                initial[0] = initialBiasX;
                initial[1] = initialBiasY;
                initial[2] = initialBiasZ;

                initial[3] = initialSx;
                initial[4] = initialSy;
                initial[5] = initialSz;

                initial[6] = initialMxy;
                initial[7] = initialMxz;
                initial[8] = initialMyx;
                initial[9] = initialMyz;
                initial[10] = initialMzx;
                initial[11] = initialMzy;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 2971
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 3283
final double initialBiasX, final double initialBiasY, final double initialBiasZ,
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy,
            final KnownPositionAccelerometerCalibratorListener listener) {
        this(commonAxisUsed, initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
                initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy, listener);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBiasX   initial x-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialBiasY   initial y-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialBiasZ   initial z-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialSx      initial x scaling factor.
     * @param initialSy      initial y scaling factor.
     * @param initialSz      initial z scaling factor.
     * @param initialMxy     initial x-y cross coupling error.
     * @param initialMxz     initial x-z cross coupling error.
     * @param initialMyx     initial y-x cross coupling error.
     * @param initialMyz     initial y-z cross coupling error.
     * @param initialMzx     initial z-x cross coupling error.
     * @param initialMzy     initial z-y cross coupling error.
     */
    public KnownPositionAccelerometerCalibrator(
            final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double initialBiasX, final double initialBiasY,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 848
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 961
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 908
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 332
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 969
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 915
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 798
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 913
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 861
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 283
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 920
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 867
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1699
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1868
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1771
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 533
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1888
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1721
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1886
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1790
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 552
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1906
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1812
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 874
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 986
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 933
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 357
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 995
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 942
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 855
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1788
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 337
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1821
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 809
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1740
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 289
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1772
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1710
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3592
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 537
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3657
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1727
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3612
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 556
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3675
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 880
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1813
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 362
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1845
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1052
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1048
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1007
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 999
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2141
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2138
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2160
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2156
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1077
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1073
innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
            throw new CalibrationException(e);
        } finally {
            running = false;
        }
    }

    /**
     * Returns method being used for robust estimation.
     *
     * @return method being used for robust estimation.
     */
    @Override
    public RobustEstimatorMethod getMethod() {
        return RobustEstimatorMethod.LMEDS;
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3727
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3796
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences   collection of sequences containing timestamped body
     *                    kinematics measurements.
     * @param initialBias initial gyroscope bias to be used to find a solution.
     *                    This must be 3x1 and is expressed in radians per
     *                    second (rad/s).
     * @param initialMg   initial gyroscope scale factors and cross coupling
     *                    errors matrix. Must be 3x3.
     * @param initialGg   initial gyroscope G-dependent cross biases
     *                    introduced on the gyroscope by the specific forces
     *                    sensed by the accelerometer. Must be 3x3.
     * @param listener    listener to handle events raised by this
     *                    calibrator.
     * @param method      robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3221
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3295
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
            default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param sequences   collection of sequences containing timestamped body
     *                    kinematics measurements.
     * @param initialBias initial gyroscope bias to be used to find a solution.
     *                    This must be 3x1 and is expressed in radians per
     *                    second (rad/s).
     * @param initialMg   initial gyroscope scale factors and cross coupling
     *                    errors matrix. Must be 3x3.
     * @param initialGg   initial gyroscope G-dependent cross biases
     *                    introduced on the gyroscope by the specific forces
     *                    sensed by the accelerometer. Must be 3x3.
     * @param listener    listener to handle events raised by this
     *                    calibrator.
     * @param method      robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if any of the provided values does
     *                                  not have proper size.
     */
    public static RobustKnownBiasEasyGyroscopeCalibrator create(
            final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4471
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6571
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4574
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7816
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5507
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7699
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5610
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6688
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5004
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7065
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5109
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8287
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6021
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8171
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6126
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7179
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 997
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1137
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2080
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2222
private void calibrateCommonAxis() throws AlgebraException, IOException {
        // The magnetometer model is:
        // mBmeas = bm + (I + Mm) * mBtrue + w

        // Ideally a least squares solution tries to minimize noise component, so:
        // mBmeas = bm + (I + Mm) * mBtrue

        // Hence:
        // [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        // [mBmeasy] = [by]     [0  1   0]   [myx   sy  myz]    [mBtruey]
        // [mBmeasz] = [bz]     [0  0   1]   [mzx   mzy sz ]    [mBtruez]

        // where myx = mzx = mzy = 0

        // Hence:
        // [mBmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [mBtruex]
        // [mBmeasy] = [by]     [0  1   0]   [0     sy  myz]    [mBtruey]
        // [mBmeasz] = [bz]     [0  0   1]   [0     0   sz ]    [mBtruez]

        // [mBmeasx] = [bx] +   [1+sx   mxy     mxz ][mBtruex]
        // [mBmeasy]   [by]     [0      1+sy    myz ][mBtruey]
        // [mBmeasz]   [bz]     [0      0       1+sz][mBtruez]

        // mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
        // mBmeasz = bz + (1+sz) * mBtruez

        // Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
        // Reordering:
        // mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
        // mBmeasz = bz + mBtruez + sz * mBtruez

        // mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
        // mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
        // mBmeasz - mBtruez = bz + sz * mBtruez

        // [1   0   0   mBtruex  0        0        mBtruey  mBtruez  0      ][bx ] = [mBmeasx - mBtruex]
        // [0   1   0   0        mBtruey  0        0        0        mBtruez][by ]   [mBmeasy - mBtruey]
        // [0   0   1   0        0        mBtruez  0        0        0      ][bz ]   [mBmeasz - mBtruez]
        //                                                                   [sx ]
        //                                                                   [sy ]
        //                                                                   [sz ]
        //                                                                   [mxy]
        //                                                                   [mxz]
        //                                                                   [myz]

        final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
        if (magneticModel != null) {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
        } else {
            wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
        }

        final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
        final var nedFrame = new NEDFrame();
        final var earthB = new NEDMagneticFluxDensity();
        final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
        final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);

        final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
        final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1109
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1248
a.setElementAt(i, 5, bTrueZ);

            b.setElementAtIndex(i, bMeasZ - bTrueZ);
            i++;
        }

        final var unknowns = Utils.solve(a, b);

        final var bx = unknowns.getElementAtIndex(0);
        final var by = unknowns.getElementAtIndex(1);
        final var bz = unknowns.getElementAtIndex(2);
        final var sx = unknowns.getElementAtIndex(3);
        final var sy = unknowns.getElementAtIndex(4);
        final var sz = unknowns.getElementAtIndex(5);
        final var mxy = unknowns.getElementAtIndex(6);
        final var mxz = unknowns.getElementAtIndex(7);
        final var myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 7456
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 7980
tmp3.setElementAtIndex(2, bMeasZ - hardIronZ);

            tmp2.multiply(tmp3, tmp4);

            final var norm = Utils.normF(tmp4);
            final var diff = groundTruthMagneticFluxDensityNorm - norm;

            return diff * diff;

        } catch (final AlgebraException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {

        final var meas = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();

        for (final var samplesIndex : samplesIndices) {
            meas.add(this.measurements.get(samplesIndex));
        }

        try {
            final var result = new PreliminaryResult();
            result.estimatedMm = getInitialMm();
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10209
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10263
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11143
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11197
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11481
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11535
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11816
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11867
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12182
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12233
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12551
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12605
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12653
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12701
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12995
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 13043
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4822
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4876
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5164
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5218
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5344
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5398
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5521
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5572
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5623
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5674
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5728
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5782
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5830
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5878
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5926
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5974
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java 1861
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java 2103
public void copyFrom(final INSLooselyCoupledKalmanState input) {
        // copy coordinate transformation matrix
        if (input.bodyToEcefCoordinateTransformationMatrix == null) {
            bodyToEcefCoordinateTransformationMatrix = null;
        } else {
            if (bodyToEcefCoordinateTransformationMatrix == null) {
                bodyToEcefCoordinateTransformationMatrix = new Matrix(input.bodyToEcefCoordinateTransformationMatrix);
            } else {
                bodyToEcefCoordinateTransformationMatrix.copyFrom(input.bodyToEcefCoordinateTransformationMatrix);
            }
        }

        vx = input.vx;
        vy = input.vy;
        vz = input.vz;

        x = input.x;
        y = input.y;
        z = input.z;

        accelerationBiasX = input.accelerationBiasX;
        accelerationBiasY = input.accelerationBiasY;
        accelerationBiasZ = input.accelerationBiasZ;

        gyroBiasX = input.gyroBiasX;
        gyroBiasY = input.gyroBiasY;
        gyroBiasZ = input.gyroBiasZ;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 894
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 841
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 265
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 902
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 848
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 788
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1721
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 270
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1754
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 267
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 982
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 980
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 984
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 730
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 803
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 217
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 934
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 288
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1008
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 842
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 790
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 212
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 849
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 796
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 913
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 860
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 284
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 922
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 869
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 738
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1669
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 218
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1701
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 807
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1740
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 289
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1772
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 215
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 933
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 927
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 932
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 286
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1001
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 999
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1004
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3567
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3847
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
                    biasX, biasY, biasZ);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasY         known y coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param biasZ         known z coordinate of accelerometer bias expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2785
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3337
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2820
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3371
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is
     *                               expressed in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2719
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3228
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final double[] bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2752
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3261
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and
     *                       is expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3314
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3865
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3350
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3900
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3244
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3748
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3278
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3782
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 624
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 609
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 696
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 680
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
            @Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return sequences.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(sequences.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3622
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3903
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, biasX, biasY, biasZ);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, biasX, biasY, biasZ);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param measurements  list of body kinematics measurements with standard
     *                      deviations taken at different frames (positions, orientations
     *                      and velocities).
     * @param biasX         known x coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasY         known y coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param biasZ         known z coordinate of gyroscope bias expressed in radians per
     *                      second (rad/s).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5219
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6256
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7554
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8683
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5373
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6410
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7389
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8517
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
                    estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5738
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6755
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8029
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 9139
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5890
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6907
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7866
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8976
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3051
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3636
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3087
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3672
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3559
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4138
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3595
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4174
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9865
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9982
final double timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9866
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10055
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 9983
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10124
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFFrame oldFrame,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10054
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10123
final double timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException {
        try {
            navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECEF frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4475
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4592
final double timeInterval, final ECIFrame oldFrame, final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(
            final Time timeInterval, final ECIFrame oldFrame, final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4476
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4667
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(
            final Time timeInterval, final ECIFrame oldFrame, final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4593
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4737
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(),
                    oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
                    angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(
            final Time timeInterval, final ECIFrame oldFrame,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4666
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4736
final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECIFrame result) throws InertialNavigatorException {
        try {
            navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldFrame     previous ECI frame containing body position, velocity and
     *                     coordinate transformation matrix.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateECI(
            final Time timeInterval, final ECIFrame oldFrame,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 108
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 212
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1394
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1498
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2052
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2156
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2731
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2829
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3471
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3569
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4214
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4318
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4416
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4508
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4600
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4692
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5293
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5385
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5657
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5749
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated NED frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1450
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1299
public boolean getState(final INSTightlyCoupledKalmanState result) {
        if (state != null) {
            result.copyFrom(state);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets timestamp expressed in seconds since epoch time when Kalman filter state
     * was last propagated.
     *
     * @return timestamp expressed in seconds since epoch time when Kalman filter
     * state was last propagated.
     */
    public Double getLastStateTimestamp() {
        return lastStateTimestamp;
    }

    /**
     * Gets timestamp since epoch time when Kalman filter state was last propagated.
     *
     * @param result instance where timestamp since epoch time when Kalman filter
     *               state was last propagated will be stored.
     * @return true if result instance is updated, false otherwise.
     */
    public boolean getLastStateTimestampAsTime(final Time result) {
        if (lastStateTimestamp != null) {
            result.setValue(lastStateTimestamp);
            result.setUnit(TimeUnit.SECOND);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets timestamp since epoch time when Kalman filter state was last propagated.
     *
     * @return timestamp since epoch time when Kalman filter state was last
     * propagated.
     */
    public Time getLastStateTimestampAsTime() {
        return lastStateTimestamp != null ? new Time(lastStateTimestamp, TimeUnit.SECOND) : null;
    }

    /**
     * Indicates whether this estimator is running or not.
     *
     * @return true if this estimator is running, false otherwise.
     */
    public boolean isRunning() {
        return running;
    }

    /**
     * Indicates whether provided measurements are ready to
     * be used for an update.
     *
     * @param measurements measurements to be checked.
     * @return true if estimator is ready, false otherwise.
     */
    public static boolean isUpdateMeasurementsReady(final Collection<GNSSMeasurement> measurements) {
File Line
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanConfig.java 133
com/irurueta/navigation/inertial/INSTightlyCoupledKalmanConfig.java 157
public INSLooselyCoupledKalmanConfig(final INSLooselyCoupledKalmanConfig input) {
        copyFrom(input);
    }

    /**
     * Gets gyro noise PSD (Power Spectral Density) expressed in squared radians per
     * second (rad^2/s).
     *
     * @return gyro noise PSD.
     */
    public double getGyroNoisePSD() {
        return gyroNoisePSD;
    }

    /**
     * Sets gyro noise PSD (Power Spectral Density) expressed in squared radians per
     * second (rad^2/s).
     *
     * @param gyroNoisePSD gyro noise PSD.
     */
    public void setGyroNoisePSD(final double gyroNoisePSD) {
        this.gyroNoisePSD = gyroNoisePSD;
    }

    /**
     * Gets accelerometer noise PSD (Power Spectral Density) expressed in (m^2 * s^-3).
     *
     * @return accelerometer noise PSD.
     */
    public double getAccelerometerNoisePSD() {
        return accelerometerNoisePSD;
    }

    /**
     * Sets accelerometer noise PSD (Power Spectral Density) expressed in (m^2 * s^-3).
     *
     * @param accelerometerNoisePSD accelerometer noise PSD.
     */
    public void setAccelerometerNoisePSD(final double accelerometerNoisePSD) {
        this.accelerometerNoisePSD = accelerometerNoisePSD;
    }

    /**
     * Gets accelerometer bias random walk PSD (Power Spectral Density) expressed
     * in (m^2 * s^-5).
     *
     * @return accelerometer bias random walk PSD.
     */
    public double getAccelerometerBiasPSD() {
        return accelerometerBiasPSD;
    }

    /**
     * Sets accelerometer bias random walk PSD (Power Spectral Density) expressed
     * in (m^2 * s^-5).
     *
     * @param accelerometerBiasPSD accelerometer bias random walk PSD.
     */
    public void setAccelerometerBiasPSD(final double accelerometerBiasPSD) {
        this.accelerometerBiasPSD = accelerometerBiasPSD;
    }

    /**
     * Gets gyro bias random walk PSD (Power Spectral Density) expressed in
     * (rad^2 * s^-3).
     *
     * @return gyro bias random walk PSD.
     */
    public double getGyroBiasPSD() {
        return gyroBiasPSD;
    }

    /**
     * Sets gyro bias random walk PSD (Power Spectral Density) expressed in
     * (rad^2 * s^-3).
     *
     * @param gyroBiasPSD gyro bias random walk PSD.
     */
    public void setGyroBiasPSD(final double gyroBiasPSD) {
        this.gyroBiasPSD = gyroBiasPSD;
    }

    /**
     * Gets position measurement noise SD (Standard Deviation) per axis expressed
     * in meters (m).
     *
     * @return position measurement noise SD.
     */
    public double getPositionNoiseSD() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6266
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6349
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6438
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3403
private double evaluateGeneral(final double[] params) throws EvaluationException {
        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java 1510
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java 1550
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 2347
b.setElementAtIndex(i, fMeasZ - fTrueZ - biasZ);
            i++;
        }

        final var unknowns = Utils.solve(a, b);

        final var sx = unknowns.getElementAtIndex(0);
        final var sy = unknowns.getElementAtIndex(1);
        final var sz = unknowns.getElementAtIndex(2);
        final var mxy = unknowns.getElementAtIndex(3);
        final var mxz = unknowns.getElementAtIndex(4);
        final var myx = unknowns.getElementAtIndex(5);
        final var myz = unknowns.getElementAtIndex(6);
        final var mzx = unknowns.getElementAtIndex(7);
        final var mzy = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java 913
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java 957
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 1111
b.setElementAtIndex(i, fMeasZ - fTrueZ);
            i++;
        }

        final var unknowns = Utils.solve(a, b);

        final var bx = unknowns.getElementAtIndex(0);
        final var by = unknowns.getElementAtIndex(1);
        final var bz = unknowns.getElementAtIndex(2);
        final var sx = unknowns.getElementAtIndex(3);
        final var sy = unknowns.getElementAtIndex(4);
        final var sz = unknowns.getElementAtIndex(5);
        final var mxy = unknowns.getElementAtIndex(6);
        final var mxz = unknowns.getElementAtIndex(7);
        final var myz = unknowns.getElementAtIndex(8);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 844
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 957
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 904
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 328
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 965
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 911
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 741
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 851
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 726
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1784
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 333
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1817
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1047
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1043
inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 818
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 763
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 826
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 770
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 794
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 740
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 801
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 746
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1695
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1864
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1767
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 529
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1884
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1786
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1411
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1706
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1381
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3588
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 533
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3653
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2136
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2133
inliersData = null;
            innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 5176
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 2931
protected double computeError(final StandardDeviationFrameBodyKinematics measurement, final Matrix preliminaryMa) {
        // We know that measured specific force is:
        // fmeas = ba + (I + Ma) * ftrue

        // Hence:
        // [fmeasx] = [bx] + ( [1  0   0] + [sx    mxy mxz])   [ftruex]
        // [fmeasy]   [by]     [0  1   0]   [myx   sy  myz]    [ftruey]
        // [fmeasz]   [bz]     [0  0   1]   [mzx   mzy sz ]    [ftruez]

        final var measuredKinematics = measurement.getKinematics();
        final var ecefFrame = measurement.getFrame();
        final var previousEcefFrame = measurement.getPreviousFrame();
        final var timeInterval = measurement.getTimeInterval();

        final var expectedKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
                previousEcefFrame);

        final var fMeasX1 = measuredKinematics.getFx();
        final var fMeasY1 = measuredKinematics.getFy();
        final var fMeasZ1 = measuredKinematics.getFz();

        final var fTrueX = expectedKinematics.getFx();
        final var fTrueY = expectedKinematics.getFy();
        final var fTrueZ = expectedKinematics.getFz();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 5782
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 6324
tmp3.setElementAtIndex(2, fmeasZ - biasZ);

            tmp2.multiply(tmp3, tmp4);

            final var norm = Utils.normF(tmp4);
            final var diff = groundTruthGravityNorm - norm;

            return diff * diff;

        } catch (final AlgebraException e) {
            return Double.MAX_VALUE;
        }
    }

    /**
     * Computes a preliminary solution for a subset of samples picked by a robust estimator.
     *
     * @param samplesIndices indices of samples picked by the robust estimator.
     * @param solutions      list where estimated preliminary solution will be stored.
     */
    protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {

        final var meas = new ArrayList<StandardDeviationBodyKinematics>();

        for (final var samplesIndex : samplesIndices) {
            meas.add(measurements.get(samplesIndex));
        }

        try {
            final var result = new PreliminaryResult();
            result.estimatedMa = getInitialMa();
File Line
com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java 1724
com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java 225
com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java 182
com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java 202
this(position, nedC, convertTime(date), magneticModel, listener);
    }


    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples expressed in seconds (s).
     *
     * @return time interval between body kinematics samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples expressed in seconds (s).
     *
     * @param timeInterval time interval between body kinematics samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @return time interval between body kinematics samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @param timeInterval time interval between body kinematics samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(convertTime(timeInterval));
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4967
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4203
final var m22 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        final var g11 = params[9];
        final var g21 = params[10];
        final var g31 = params[11];

        final var g12 = params[12];
        final var g22 = params[13];
        final var g32 = params[14];

        final var g13 = params[15];
        final var g23 = params[16];
        final var g33 = params[17];

        return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4471
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7699
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5507
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6571
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5004
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8171
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6021
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7065
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
            final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 7425
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 6422
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 7945
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 6909
if (identity == null) {
                identity = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }

            if (tmp1 == null) {
                tmp1 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
            }

            if (tmp2 == null) {
                tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
            }

            if (tmp3 == null) {
                tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            if (tmp4 == null) {
                tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
            }

            identity.add(estMm, tmp1);
File Line
com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java 1592
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 6720
final var alphaSkew1 = Utils.skewMatrix(new double[]{alphaX, alphaY, alphaZ});

                if (alphaNorm > ALPHA_THRESHOLD) {
                    final var alphaNorm2 = alphaNorm * alphaNorm;
                    final var value1 = (1.0 - Math.cos(alphaNorm)) / alphaNorm2;
                    final var value2 = (1.0 - Math.sin(alphaNorm) / alphaNorm) / alphaNorm2;
                    final var tmp1 = alphaSkew1.multiplyByScalarAndReturnNew(value1);
                    final var tmp2 = alphaSkew1.multiplyByScalarAndReturnNew(value2);
                    tmp2.multiply(alphaSkew1);

                    final var tmp3 = Matrix.identity(ROWS, ROWS);
                    tmp3.add(tmp1);
                    tmp3.add(tmp2);

                    oldCbe.multiply(tmp3);
                }
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 6891
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 6938
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 6985
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7033
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7403
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7451
final double timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getInhomX(), oldPosition.getInhomY(), oldPosition.getInhomZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7797
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7845
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
                oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7882
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7918
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final BodyKinematics kinematics, final ECEFFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
                kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(), result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8128
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8175
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8449
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8493
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
                oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8804
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8848
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
                oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3410
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3458
final double timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldPosition.getInhomX(), oldPosition.getInhomY(), oldPosition.getInhomZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECI(
            final Time timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3659
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3695
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final BodyKinematics kinematics, final ECIFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
                kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
                kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(), result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECI(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17695
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18719
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17697
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19692
final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17698
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20171
final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17750
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18774
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19197
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20649
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19252
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20704
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19693
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21496
final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20170
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21495
final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20753
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20851
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20802
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20900
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21299
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21493
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21348
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21542
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 4302
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2370
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 1266
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1683
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1637
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 2363
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3446
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 1285
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 2367
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3408
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @return array containing coordinates of known bias.
     */
    @Override
    public double[] getBias() {
        final var result = new double[BodyKinematics.COMPONENTS];
        getBias(result);
        return result;
    }

    /**
     * Gets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param result instance where result data will be copied to.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void getBias(final double[] result) {
        if (result.length != BodyKinematics.COMPONENTS) {
            throw new IllegalArgumentException();
        }
        result[0] = biasX;
        result[1] = biasY;
        result[2] = biasZ;
    }

    /**
     * Sets known bias as an array.
     * Array values are expressed in meters per squared second (m/s^2).
     *
     * @param bias known bias to be set
     * @throws LockedException          if calibrator is currently running.
     * @throws IllegalArgumentException if provided array does not have length 3.
     */
    @Override
    public void setBias(final double[] bias) throws LockedException {
        if (running) {
            throw new LockedException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6266
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4909
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6438
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3403
private double evaluateGeneral(final double[] params) throws EvaluationException {
        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java 6363
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5895
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6575
fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 683
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1621
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1653
super(measurements, bias, commonAxisUsed, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4193
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4432
final double[] qualityScores, final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(bias, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(bias, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(bias, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, bias,
                    commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, bias, commonAxisUsed,
                    listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known accelerometer bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller than
     *                                  4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2719
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3273
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2821
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2955
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is
     *                               expressed in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2920
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3468
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2954
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3501
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3023
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3568
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3372
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3502
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2662
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3171
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias. This must have length 3 and is
     *                     expressed in meters per squared second (m/s^2).
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2753
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2873
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and
     *                       is expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2840
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3350
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2872
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3382
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2937
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3447
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix bias, final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3262
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3383
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3246
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3799
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3351
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3485
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3450
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3997
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3484
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4030
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3552
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4096
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3901
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4031
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3185
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3689
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial accelerometer bias to be used to find a solution.
     *                     This must have length 3 and is expressed in meters per
     *                     squared second (m/s^2).
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3279
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3400
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3367
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3871
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3399
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3903
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final Matrix initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3463
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3967
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, initialMa);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3783
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3904
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java 1725
com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java 109
com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java 144
}


    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples expressed in seconds (s).
     *
     * @return time interval between body kinematics samples.
     */
    public double getTimeInterval() {
        return timeInterval;
    }

    /**
     * Sets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples expressed in seconds (s).
     *
     * @param timeInterval time interval between body kinematics samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final double timeInterval) throws LockedException {
        if (running) {
            throw new LockedException();
        }

        if (timeInterval < 0.0) {
            throw new IllegalArgumentException();
        }

        this.timeInterval = timeInterval;
    }

    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @return time interval between body kinematics samples.
     */
    public Time getTimeIntervalAsTime() {
        return new Time(timeInterval, TimeUnit.SECOND);
    }

    /**
     * Gets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @param result instance where time interval will be stored.
     */
    public void getTimeIntervalAsTime(final Time result) {
        result.setValue(timeInterval);
        result.setUnit(TimeUnit.SECOND);
    }

    /**
     * Sets time interval between body kinematics (IMU acceleration + gyroscope)
     * samples.
     *
     * @param timeInterval time interval between body kinematics samples.
     * @throws LockedException if estimator is currently running.
     */
    public void setTimeInterval(final Time timeInterval) throws LockedException {
        setTimeInterval(convertTime(timeInterval));
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5001
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6349
private double evaluateGeneral(final int i, final double[] params) throws EvaluationException {

        final var bx = params[0];
        final var by = params[1];
        final var bz = params[2];

        final var m11 = params[3];
        final var m21 = params[4];
        final var m31 = params[5];

        final var m12 = params[6];
        final var m22 = params[7];
        final var m32 = params[8];

        final var m13 = params[9];
        final var m23 = params[10];
        final var m33 = params[11];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3360
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4683
b = invInitialM.multiplyAndReturnNew(bg);

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Before and after normalized gravity versors
                return 2 * BodyKinematics.COMPONENTS;
            }

            @Override
            public double[] createInitialParametersArray() {
                final var initial = new double[GENERAL_UNKNOWNS_AND_CROSS_BIASES];

                // cross coupling errors M
                final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                for (var i = 0; i < num; i++) {
                    initial[i] = initialM.getElementAtIndex(i);
                }

                // g-dependent cross biases G
                for (int i = 0, j = num; i < num; i++, j++) {
                    initial[j] = initialG.getElementAtIndex(i);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5799
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6675
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3500
fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, biasX);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3991
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4376
System.arraycopy(buffer, 0, initial, 9, num);

                return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                // Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
                // Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
                // Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz,
                // sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23,
                // g31, g32, and g33 is:

                // d(Ωmeasx)/d(bx) = 1.0
                // d(Ωmeasx)/d(by) = 0.0
                // d(Ωmeasx)/d(bz) = 0.0
                // d(Ωmeasx)/d(sx) = Ωtruex
                // d(Ωmeasx)/d(sy) = 0.0
                // d(Ωmeasx)/d(sz) = 0.0
                // d(Ωmeasx)/d(mxy) = Ωtruey
                // d(Ωmeasx)/d(mxz) = Ωtruez
                // d(Ωmeasx)/d(myz) = 0.0
                // d(Ωmeasx)/d(g11) = ftruex
                // d(Ωmeasx)/d(g12) = ftruey
                // d(Ωmeasx)/d(g13) = ftruez
                // d(Ωmeasx)/d(g21) = 0.0
                // d(Ωmeasx)/d(g22) = 0.0
                // d(Ωmeasx)/d(g23) = 0.0
                // d(Ωmeasx)/d(g31) = 0.0
                // d(Ωmeasx)/d(g32) = 0.0
                // d(Ωmeasx)/d(g33) = 0.0

                // d(Ωmeasy)/d(bx) = 0.0
                // d(Ωmeasy)/d(by) = 1.0
                // d(Ωmeasy)/d(bz) = 0.0
                // d(Ωmeasy)/d(sx) = 0.0
                // d(Ωmeasy)/d(sy) = Ωtruey
                // d(Ωmeasy)/d(sz) = 0.0
                // d(Ωmeasy)/d(mxy) = 0.0
                // d(Ωmeasy)/d(mxz) = 0.0
                // d(Ωmeasy)/d(myz) = Ωtruez
                // d(Ωmeasx)/d(g11) = 0.0
                // d(Ωmeasx)/d(g12) = 0.0
                // d(Ωmeasx)/d(g13) = 0.0
                // d(Ωmeasx)/d(g21) = ftruex
                // d(Ωmeasx)/d(g22) = ftruey
                // d(Ωmeasx)/d(g23) = ftruez
                // d(Ωmeasx)/d(g31) = 0.0
                // d(Ωmeasx)/d(g32) = 0.0
                // d(Ωmeasx)/d(g33) = 0.0

                // d(Ωmeasz)/d(bx) = 0.0
                // d(Ωmeasz)/d(by) = 0.0
                // d(Ωmeasz)/d(bz) = 1.0
                // d(Ωmeasz)/d(sx) = 0.0
                // d(Ωmeasz)/d(sy) = 0.0
                // d(Ωmeasz)/d(sz) = Ωtruez
                // d(Ωmeasz)/d(mxy) = 0.0
                // d(Ωmeasz)/d(mxz) = 0.0
                // d(Ωmeasz)/d(myz) = 0.0
                // d(Ωmeasx)/d(g11) = 0.0
                // d(Ωmeasx)/d(g12) = 0.0
                // d(Ωmeasx)/d(g13) = 0.0
                // d(Ωmeasx)/d(g21) = 0.0
                // d(Ωmeasx)/d(g22) = 0.0
                // d(Ωmeasx)/d(g23) = 0.0
                // d(Ωmeasx)/d(g31) = ftruex
                // d(Ωmeasx)/d(g32) = ftruey
                // d(Ωmeasx)/d(g33) = ftruez

                final var bx = params[0];
                final var by = params[1];
                final var bz = params[2];

                final var sx = params[3];
                final var sy = params[4];
                final var sz = params[5];

                final var mxy = params[6];
                final var mxz = params[7];
                final var myz = params[8];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3870
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4636
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4245
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4482
final double[] qualityScores, final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(bias, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(bias, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(bias, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, bias, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, bias, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known gyroscope bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller
     *                                  than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3380
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4152
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
            final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
                    sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4748
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5784
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 7017
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8145
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 4873
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 5910
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 6881
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 8009
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
                    position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5282
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6299
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7499
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8607
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
            final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 5401
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 6418
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 7369
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 8477
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
            final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
                    measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
            case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 537
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3621
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1722
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3585
public void setListener(final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return magneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     * If not provided a default model will be loaded internally.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.magneticModel = magneticModel;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if calibration fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2910
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 3043
final double[] hardIron,
            final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 3590
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 3731
final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param hardIron                           known hard-iron.
     * @param listener                           listener to handle events raised by this calibrator.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 4243
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 4393
final double[] hardIron,
            final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided quality
     *                                  scores length is smaller than 10 samples.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2982
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3567
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3088
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3229
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3192
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3777
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3228
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3813
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3302
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3887
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix hardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param initialMm    initial soft-iron matrix containing scale factors
     *                     and cross coupling errors.
     * @param listener     listener to handle events raised by this calibrator.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3673
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3814
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 3421
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 3554
final double[] initialHardIron,
            final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4101
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4242
final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param commonAxisUsed                     indicates whether z-axis is assumed to be common
     *                                           for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron                    initial hard-iron to find a solution.
     * @param listener                           listener to handle events raised by this calibrator.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4755
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4905
final double[] initialHardIron,
            final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided quality
     *                                  scores length is smaller than 10 samples.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3491
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4070
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3596
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3737
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3700
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4279
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3736
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4315
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final Matrix initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3810
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4389
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final Matrix initialHardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, initialMm);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4175
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4316
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2073
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2117
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3111
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4040
estimatedMa.setElementAt(2, 2, sz);

        estimatedCovariance = fitter.getCovar();

        // propagate covariance matrix so that all parameters are taken into
        // account in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy

        // We define a lineal function mapping original parameters for the common
        // axis case to the general case
        // [sx'] = [1  0  0  0  0  0][sx]
        // [sy']   [0  1  0  0  0  0][sy]
        // [sz']   [0  0  1  0  0  0][sz]
        // [mxy']  [0  0  0  1  0  0][mxy]
        // [mxz']  [0  0  0  0  1  0][mxz]
        // [myx']  [0  0  0  0  0  0][myz]
        // [myz']  [0  0  0  0  0  1]
        // [mzx']  [0  0  0  0  0  0]
        // [mzy']  [0  0  0  0  0  0]

        // As defined in com.irurueta.statistics.MultivariateNormalDist,
        // if we consider the jacobian of the lineal application the matrix shown
        // above, then covariance can be propagated as follows
        final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
        jacobian.setElementAt(5, 5, 0.0);
        jacobian.setElementAt(6, 5, 1.0);

        // propagated covariance is J * Cov * J'
        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 2729
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 3010
final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) {
        this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz,
                initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param biasX          x-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param biasY          y-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param biasZ          z-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param initialSx      initial x scaling factor.
     * @param initialSy      initial y scaling factor.
     * @param initialSz      initial z scaling factor.
     * @param initialMxy     initial x-y cross coupling error.
     * @param initialMxz     initial x-z cross coupling error.
     * @param initialMyx     initial y-x cross coupling error.
     * @param initialMyz     initial y-z cross coupling error.
     * @param initialMzx     initial z-x cross coupling error.
     * @param initialMzy     initial z-y cross coupling error.
     * @param listener       listener to handle events raised by this calibrator.
     */
    public KnownBiasAndPositionAccelerometerCalibrator(
            final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3857
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4604
estimatedMa.setElementAt(2, 2, sz);

        estimatedCovariance = fitter.getCovar();

        // propagate covariance matrix so that all parameters are taken into
        // account in the order: bx, by, bz, sx, sy, sz, mxy, mxz, myx,
        // myz, mzx, mzy.

        // We define a lineal function mapping original parameters for the common
        // axis case to the general case
        // [bx'] = [1  0  0  0  0  0  0  0  0][bx]
        // [by']   [0  1  0  0  0  0  0  0  0][by]
        // [bz']   [0  0  1  0  0  0  0  0  0][bz]
        // [sx']   [0  0  0  1  0  0  0  0  0][sx]
        // [sy']   [0  0  0  0  1  0  0  0  0][sy]
        // [sz']   [0  0  0  0  0  1  0  0  0][sz]
        // [mxy']  [0  0  0  0  0  0  1  0  0][mxy]
        // [mxz']  [0  0  0  0  0  0  0  1  0][mxz]
        // [myx']  [0  0  0  0  0  0  0  0  0][myz]
        // [myz']  [0  0  0  0  0  0  0  0  1]
        // [mzx']  [0  0  0  0  0  0  0  0  0]
        // [mzy']  [0  0  0  0  0  0  0  0  0]

        // As defined in com.irurueta.statistics.MultivariateNormalDist,
        // if we consider the jacobian of the lineal application the matrix shown
        // above, then covariance can be propagated as follows
        final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
        jacobian.setElementAt(8, 8, 0.0);
        jacobian.setElementAt(9, 8, 1.0);
        // propagated covariance is J * Cov * J'
        final var jacobianTrans = jacobian.transposeAndReturnNew();
        jacobian.multiply(estimatedCovariance);
        jacobian.multiply(jacobianTrans);
        estimatedCovariance = jacobian;
        estimatedChiSq = fitter.getChisq();
        estimatedMse = fitter.getMse();
    }

    /**
     * Internal method to perform general calibration.
     *
     * @throws AlgebraException                         if there are numerical errors.
     * @throws FittingException                         if no convergence to solution is found.
     * @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
     */
    private void calibrateGeneral() throws AlgebraException, FittingException,
            com.irurueta.numerical.NotReadyException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 3016
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 3325
final double initialBiasZ, final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) {
        this(measurements, commonAxisUsed, initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
                initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBiasX   initial x-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialBiasY   initial y-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialBiasZ   initial z-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialSx      initial x scaling factor.
     * @param initialSy      initial y scaling factor.
     * @param initialSz      initial z scaling factor.
     * @param initialMxy     initial x-y cross coupling error.
     * @param initialMxz     initial x-z cross coupling error.
     * @param initialMyx     initial y-x cross coupling error.
     * @param initialMyz     initial y-z cross coupling error.
     * @param initialMzx     initial z-x cross coupling error.
     * @param initialMzy     initial z-y cross coupling error.
     * @param listener       listener to handle events raised by this calibrator.
     */
    public KnownPositionAccelerometerCalibrator(
            final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
            final boolean commonAxisUsed, final double initialBiasX, final double initialBiasY,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2610
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2843
final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3638
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 3913
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
            final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
                    commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
                    commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
                    commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasY          known y coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param biasZ          known z coordinate of accelerometer bias expressed in meters per
     *                       squared second (m/s^2).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2856
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3535
final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 2989
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3406
final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3901
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4706
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4059
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4551
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2786
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3415
final boolean commonAxisUsed, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 2905
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3295
final boolean commonAxisUsed, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param bias         known accelerometer bias.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3761
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4512
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3910
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4363
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3387
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4064
final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3519
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 3936
final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param initialMa              initial scale factors and cross coupling errors matrix.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3 or
     *                                  if provided gravity norm value is negative.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4427
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5232
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4587
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5077
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3313
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3936
final boolean commonAxisUsed, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
                    measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3432
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 3817
final boolean commonAxisUsed, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBias  initial bias to find a solution.
     * @param initialMa    initial scale factors and cross coupling errors matrix.
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
     *                                  scaling and coupling error matrix is not 3x3.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4273
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5015
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4426
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4865
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4967
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5607
final var m22 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        final var g11 = params[9];
        final var g21 = params[10];
        final var g31 = params[11];

        final var g12 = params[12];
        final var g22 = params[13];
        final var g32 = params[14];

        final var g13 = params[15];
        final var g23 = params[16];
        final var g33 = params[17];

        return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 5128
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5801
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5897
m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            b.setElementAtIndex(0, bx);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 4203
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 6404
final var m32 = params[5];

        final var m13 = params[6];
        final var m23 = params[7];
        final var m33 = params[8];

        final var g11 = params[9];
        final var g21 = params[10];
        final var g31 = params[11];

        final var g12 = params[12];
        final var g22 = params[13];
        final var g32 = params[14];

        final var g13 = params[15];
        final var g23 = params[16];
        final var g33 = params[17];

        return evaluate(i, m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java 742
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java 727
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 334
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 333
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1052
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1051
innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java 1412
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java 1382
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 537
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 535
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2146
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2142
innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException | AlgebraException e) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2678
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2907
final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
                    listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3697
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 3969
final double biasZ, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, biasX, biasY, biasZ, commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, biasX, biasY, biasZ, commonAxisUsed);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param biasX          known x coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasY          known y coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param biasZ          known z coordinate of gyroscope bias expressed in radians per
     *                       second (rad/s).
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2734
com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java 3518
invM.multiply(bm, b);

            invM.multiply(bmeas, btrue);
            btrue.subtract(b);

            final var norm = Utils.normF(btrue);
            return norm * norm;

        } catch (final AlgebraException e) {
            throw new EvaluationException(e);
        }
    }

    /**
     * Converts magnetic flux density value and unit to Teslas.
     *
     * @param value magnetic flux density value.
     * @param unit  unit of magnetic flux density value.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
        return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
    }

    /**
     * Converts magnetic flux density instance to Teslas.
     *
     * @param magneticFluxDensity magnetic flux density instance to be converted.
     * @return converted value.
     */
    private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
        return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
    }
}
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3124
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3850
final boolean commonAxisUsed, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3265
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 3709
final boolean commonAxisUsed, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position     position where body magnetic flux density measurements
     *                     have been taken.
     * @param measurements collection of body magnetic flux density
     *                     measurements with standard deviation of
     *                     magnetometer measurements taken at the same
     *                     position with zero velocity and unknown different
     *                     orientations.
     * @param hardIron     known hard-iron.
     * @param initialMm    initial soft-iron matrix containing scale factors
     *                     and cross coupling errors.
     * @param method       robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4430
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5263
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4589
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5101
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3632
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4352
final boolean commonAxisUsed, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 3773
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4211
final boolean commonAxisUsed, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param initialMm       initial soft-iron matrix containing scale factors
     *                        and cross coupling errors.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if soft-iron matrix is not
     *                                  3x3.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4918
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5747
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5077
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5584
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2072
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8803
public void navigate(
            final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2116
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8847
public void navigate(
            final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13491
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15343
final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(),
                oldPosition.getHeight(), oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13492
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15725
final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(),
                oldPosition.getHeight(), oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13591
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14398
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13593
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15439
final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13594
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15820
final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15344
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16665
final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(),
                oldC, oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
                result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 15724
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16664
final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5332
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2717
fmeas.setElementAtIndex(2, fmeasZ);

            m.setElementAt(0, 0, m11);
            m.setElementAt(1, 0, m21);
            m.setElementAt(2, 0, m31);

            m.setElementAt(0, 1, m12);
            m.setElementAt(1, 1, m22);
            m.setElementAt(2, 1, m32);

            m.setElementAt(0, 2, m13);
            m.setElementAt(1, 2, m23);
            m.setElementAt(2, 2, m33);

            Utils.inverse(m, invM);

            // b = M^-1*ba
            invM.multiply(ba, b);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2718
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 2940
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3055
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3188
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3680
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3644
}

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }

        if (!isReady()) {
            throw new NotReadyException();
        }

        try {
            running = true;

            if (listener != null) {
                listener.onCalibrateStart(this);
            }

            if (commonAxisUsed) {
                calibrateCommonAxis();
            } else {
                calibrateGeneral();
            }

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 845
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 958
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 905
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java 329
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 966
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 912
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 852
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 1785
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java 334
com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java 1818
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java 334
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 333
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1049
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1052
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1045
com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 1051
innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            innerEstimator.setStopThreshold(stopThreshold);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java 1696
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1865
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 1768
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java 530
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java 1885
com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java 1787
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java 1707
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java 3589
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java 534
com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java 3654
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java 537
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java 535
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2138
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2146
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2135
com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java 2142
innerEstimator.setUseInlierThresholds(true);
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3861
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4512
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3726
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4325
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4386
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5037
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4237
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4829
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3613
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4929
initial[i] = initialB.getElementAtIndex(i);
                }

                // upper diagonal cross coupling errors M
                var k = BodyKinematics.COMPONENTS;
                for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                // g-dependent cross biases G
                final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
                for (int i = 0, j = k; i < num; i++, j++) {
                    initial[j] = initialG.getElementAtIndex(i);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 3665
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3415
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4742
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4984
final var m22 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var g11 = result[9];
        final var g21 = result[10];
        final var g31 = result[11];

        final var g12 = result[12];
        final var g22 = result[13];
        final var g32 = result[14];

        final var g13 = result[15];
        final var g23 = result[16];
        final var g33 = result[17];

        final var b = new Matrix(BodyKinematics.COMPONENTS, 1);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4129
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4931
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3639
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4451
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4389
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5060
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4877
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5543
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 3154
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13348
final var skewAlpha = Utils.skewMatrix(alphaIbb);

            // Obtain coordinate transformation matrix from the new attitude to the old
            // using Rodrigues' formula, (5.73)
            final var cNewOld = Matrix.identity(ROWS, ROWS);
            if (magAlpha > ALPHA_THRESHOLD) {
                final var magAlpha2 = magAlpha * magAlpha;
                final var value1 = Math.sin(magAlpha) / magAlpha;
                final var value2 = (1.0 - Math.cos(magAlpha)) / magAlpha2;

                final var tmp1 = skewAlpha.multiplyByScalarAndReturnNew(value1);
                final var tmp2 = skewAlpha.multiplyByScalarAndReturnNew(value2);
                tmp2.multiply(skewAlpha);

                cNewOld.add(tmp1);
                cNewOld.add(tmp2);
            } else {
                cNewOld.add(skewAlpha);
            }
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 160
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 264
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1446
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 1550
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2104
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2208
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2780
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 2878
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3520
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 3618
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4266
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4370
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4462
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4554
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4646
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 4738
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5339
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5431
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5703
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 5795
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 13437
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14349
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 14813
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16060
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16159
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16256
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16616
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 16804
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateNED(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java 1348
com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java 1225
}

    /**
     * Gets last provided user kinematics containing applied specific force and
     * angular rates resolved in body axes.
     *
     * @return last provided user kinematics.
     */
    public BodyKinematics getKinematics() {
        return kinematics != null ? new BodyKinematics(kinematics) : null;
    }

    /**
     * Gets last provided user kinematics containing applied specific force and
     * angular rates resolved in body axes.
     *
     * @param result instance where last provided body kinematics will be stored.
     * @return true if provided result instance was updated, false otherwise.
     */
    public boolean getKinematics(final BodyKinematics result) {
        if (kinematics != null) {
            result.copyFrom(kinematics);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets corrected kinematics which are the last provided user kinematics after
     * removal of the biases estimated by the Kalman filter.
     *
     * @return corrected kinematics.
     * @see #getKinematics()
     */
    public BodyKinematics getCorrectedKinematics() {
        return correctedKinematics != null ? new BodyKinematics(correctedKinematics) : null;
    }

    /**
     * Gets corrected kinematics which are the last provided user kinematics after
     * removal of the biases estimated by the Kalman filter.
     *
     * @param result instance where corrected body kinematics will be stored.
     * @return true if provided result instance was updated, false otherwise.
     */
    public boolean getCorrectedKinematics(final BodyKinematics result) {
        if (correctedKinematics != null) {
            result.copyFrom(correctedKinematics);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Gets current estimation containing user ECEF position, user ECEF velocity,
     * clock offset and clock drift.
     *
     * @return current estimation containing user ECEF position, user ECEF velocity,
     * clock offset and clock drift.
     */
    public GNSSEstimation getEstimation() {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3722
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3978
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 3993
com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java 4378
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4468
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4726
return initial;
            }

            @Override
            public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
                                 final Matrix jacobian) {
                // We know that:
                // fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
                // fmeasy = by + ftruey + sy * ftruey + myz * ftruez
                // fmeasz = bz + ftruez + sz * ftruez

                // Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
                // sz, mxy, mxz, myz

                // d(fmeasx)/d(bx) = 1.0
                // d(fmeasx)/d(by) = 0.0
                // d(fmeasx)/d(bz) = 0.0
                // d(fmeasx)/d(sx) = ftruex
                // d(fmeasx)/d(sy) = 0.0
                // d(fmeasx)/d(sz) = 0.0
                // d(fmeasx)/d(mxy) = ftruey
                // d(fmeasx)/d(mxz) = ftruez
                // d(fmeasx)/d(myz) = 0.0

                // d(fmeasy)/d(bx) = 0.0
                // d(fmeasy)/d(by) = 1.0
                // d(fmeasy)/d(bz) = 0.0
                // d(fmeasy)/d(sx) = 0.0
                // d(fmeasy)/d(sy) = ftruey
                // d(fmeasy)/d(sz) = 0.0
                // d(fmeasy)/d(mxy) = 0.0
                // d(fmeasy)/d(mxz) = 0.0
                // d(fmeasy)/d(myz) = ftruez

                // d(fmeasz)/d(bx) = 0.0
                // d(fmeasz)/d(by) = 0.0
                // d(fmeasz)/d(bz) = 1.0
                // d(fmeasz)/d(sx) = 0.0
                // d(fmeasz)/d(sy) = 0.0
                // d(fmeasz)/d(sz) = ftruez
                // d(fmeasz)/d(mxy) = 0.0
                // d(fmeasz)/d(mxz) = 0.0
                // d(fmeasz)/d(myz) = 0.0

                final var bx = params[0];
                final var by = params[1];
                final var bz = params[2];

                final var sx = params[3];
                final var sy = params[4];
                final var sz = params[5];

                final var mxy = params[6];
                final var mxz = params[7];
                final var myz = params[8];
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4704
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5287
for (int i = 0, j = num; i < num; i++, j++) {
                    initial[j] = initG.getElementAtIndex(i);
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {

                measAngularRateX = point[0];
                measAngularRateY = point[1];
                measAngularRateZ = point[2];

                fmeasX = point[3];
                fmeasY = point[4];
                fmeasZ = point[5];

                gradientEstimator.gradient(params, derivatives);

                return evaluateGeneralWitGDependentCrossBiases(params);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java 537
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3621
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java 1722
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 3585
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 1364
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 1328
public void setListener(final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
            throws LockedException {
        if (running) {
            throw new LockedException();
        }

        this.listener = listener;
    }

    /**
     * Gets minimum number of required measurements.
     *
     * @return minimum number of required measurements.
     */
    @Override
    public int getMinimumRequiredMeasurements() {
        return MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is ready to start the estimator.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
        return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
    }

    /**
     * Indicates whether calibrator is currently running or no.
     *
     * @return true if calibrator is running, false otherwise.
     */
    @Override
    public boolean isRunning() {
        return running;
    }

    /**
     * Gets Earth's magnetic model.
     *
     * @return Earth's magnetic model or null if not provided.
     */
    public WorldMagneticModel getMagneticModel() {
        return magneticModel;
    }

    /**
     * Sets Earth's magnetic model.
     * If not provided a default model will be loaded internally.
     *
     * @param magneticModel Earth's magnetic model to be set.
     * @throws LockedException if calibrator is currently running.
     */
    public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        this.magneticModel = magneticModel;
    }
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 7118
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8663
final ECEFVelocity oldVelocity, final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC, oldVelocity,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static void navigateECEF(
            final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final BodyKinematics kinematics, final ECEFFrame result) throws InertialNavigatorException,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 6657
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 6761
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 7947
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 8051
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 8605
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 8711
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 9288
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 9386
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 10028
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 10126
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 10771
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 10877
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 10977
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11071
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11165
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11259
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
                oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
                accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldSpeedN    previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedE    previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param oldSpeedD    previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11861
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 11953
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude angle.
     * @param oldLongitude previous longitude angle.
     * @param oldHeight    previous height.
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12225
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 12317
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldLatitude  previous latitude expressed in radians (rad).
     * @param oldLongitude previous longitude expressed in radians (rad).
     * @param oldHeight    previous height expressed in meters (m).
     * @param oldC         previous body-to-NED coordinate transformation.
     * @param oldVn        previous velocity north-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe        previous velocity east-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd        previous velocity down-coordinate of body frame with respect ECEF frame,
     *                     resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public NEDFrame navigateAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17695
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18774
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17750
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18719
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17804
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17858
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17909
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17960
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18825
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 18876
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19197
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20704
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19252
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20649
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19306
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19360
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19793
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19844
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19892
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 19940
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20271
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20322
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20370
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20418
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in meters per squared second (m/s^2).
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20753
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20900
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20802
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20851
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
                fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldLatitude       previous latitude expressed in radians (rad).
     * @param oldLongitude      previous longitude expressed in radians (rad).
     * @param oldHeight         previous height expressed in meters (m).
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20948
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 20996
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldSpeedN         previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedE         previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param oldSpeedD         previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21041
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21086
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVelocity       previous velocity of body frame with respect ECEF frame resolved
     *                          along north, east and down axes.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21299
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21542
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs.
     * @param oldLatitude       previous latitude angle.
     * @param oldLongitude      previous longitude angle.
     * @param oldHeight         previous height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21348
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21493
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
            final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21396
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 21444
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVn, final double oldVe, final double oldVd,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final double accuracyThreshold) throws InertialNavigatorException,
            InvalidSourceAndDestinationFrameTypeException {
        final var result = new NEDFrame();
        navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        return result;
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldPosition       previous curvilinear position expressed in terms of latitude,
     *                          longitude and height.
     * @param oldC              previous body-to-NED coordinate transformation.
     * @param oldVn             previous velocity north-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVe             previous velocity east-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param oldVd             previous velocity down-coordinate of body frame with respect ECEF frame,
     *                          resolved along NED-frame axes and expressed in meters per second (m/s).
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
     * @return estimated NED frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-NED-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static NEDFrame navigateNEDAndReturnNew(
            final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3077
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4006
jacobian.setElementAt(2, 2, ftruez);
                jacobian.setElementAt(2, 3, 0.0);
                jacobian.setElementAt(2, 4, 0.0);
                jacobian.setElementAt(2, 5, 0.0);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var sx = result[0];
        final var sy = result[1];
        final var sz = result[2];

        final var mxy = result[3];
        final var mxz = result[4];
        final var myz = result[5];

        if (estimatedMa == null) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 2543
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 2580
this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
                initialMyz, initialMzx, initialMzy);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param biasX        x-coordinate of accelerometer bias.
     *                     This is expressed in meters per squared second (m/s^2).
     * @param biasY        y-coordinate of accelerometer bias.
     *                     This is expressed in meters per squared second (m/s^2).
     * @param biasZ        z-coordinate of accelerometer bias.
     *                     This is expressed in meters per squared second (m/s^2).
     * @param initialSx    initial x scaling factor.
     * @param initialSy    initial y scaling factor.
     * @param initialSz    initial z scaling factor.
     * @param initialMxy   initial x-y cross coupling error.
     * @param initialMxz   initial x-z cross coupling error.
     * @param initialMyx   initial y-x cross coupling error.
     * @param initialMyz   initial y-z cross coupling error.
     * @param initialMzx   initial z-x cross coupling error.
     * @param initialMzy   initial z-y cross coupling error.
     */
    public KnownBiasAndPositionAccelerometerCalibrator(
            final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
            final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 2615
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 2905
final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
            final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy,
            final KnownBiasAndPositionAccelerometerCalibrationListener listener) {
        this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz,
                initialMyx, initialMyz, initialMzx, initialMzy, listener);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param biasX          x-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param biasY          y-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param biasZ          z-coordinate of accelerometer bias.
     *                       This is expressed in meters per squared second (m/s^2).
     * @param initialSx      initial x scaling factor.
     * @param initialSy      initial y scaling factor.
     * @param initialSz      initial z scaling factor.
     * @param initialMxy     initial x-y cross coupling error.
     * @param initialMxz     initial x-z cross coupling error.
     * @param initialMyx     initial y-x cross coupling error.
     * @param initialMyz     initial y-z cross coupling error.
     * @param initialMzx     initial z-x cross coupling error.
     * @param initialMzy     initial z-y cross coupling error.
     */
    public KnownBiasAndPositionAccelerometerCalibrator(
            final ECEFPosition position, final boolean commonAxisUsed, final double biasX, final double biasY,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java 5250
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 5748
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 1844
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 1891
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 3860
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java 3820
com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java 3872
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 3907
}

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @return position where body kinematics measures have been taken or null if
     * not available.
     */
    public NEDPosition getNedPosition() {
        final var result = new NEDPosition();
        return getNedPosition(result) ? result : null;
    }

    /**
     * Gets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param result instance where result will be stored.
     * @return true if NED position could be computed, false otherwise.
     */
    public boolean getNedPosition(final NEDPosition result) {

        if (position != null) {
            final var velocity = new NEDVelocity();
            ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
                    position.getX(), position.getY(), position.getZ(),
                    0.0, 0.0, 0.0, result, velocity);
            return true;
        } else {
            return false;
        }
    }

    /**
     * Sets position where body kinematics measures have been taken expressed in
     * NED coordinates.
     *
     * @param position position where body kinematics measures have been taken.
     * @throws LockedException if calibrator is currently running.
     */
    public void setPosition(final NEDPosition position) throws LockedException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 2810
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 2851
this(initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
                initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position     position where body kinematics measures have been taken.
     * @param measurements collection of body kinematics measurements with standard
     *                     deviations taken at the same position with zero velocity
     *                     and unknown different orientations.
     * @param initialBiasX initial x-coordinate of accelerometer bias to be used
     *                     to find a solution. This is expressed in meters per squared
     *                     second (m/s^2).
     * @param initialBiasY initial y-coordinate of accelerometer bias to be used
     *                     to find a solution. This is expressed in meters per squared
     *                     second (m/s^2).
     * @param initialBiasZ initial z-coordinate of accelerometer bias to be used
     *                     to find a solution. This is expressed in meters per squared
     *                     second (m/s^2).
     * @param initialSx    initial x scaling factor.
     * @param initialSy    initial y scaling factor.
     * @param initialSz    initial z scaling factor.
     * @param initialMxy   initial x-y cross coupling error.
     * @param initialMxz   initial x-z cross coupling error.
     * @param initialMyx   initial y-x cross coupling error.
     * @param initialMyz   initial y-z cross coupling error.
     * @param initialMzx   initial z-x cross coupling error.
     * @param initialMzy   initial z-y cross coupling error.
     */
    public KnownPositionAccelerometerCalibrator(
            final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
            final double initialBiasX, final double initialBiasY, final double initialBiasZ,
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 2889
com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java 3207
final double initialBiasX, final double initialBiasY, final double initialBiasZ,
            final double initialSx, final double initialSy, final double initialSz,
            final double initialMxy, final double initialMxz, final double initialMyx,
            final double initialMyz, final double initialMzx, final double initialMzy,
            final KnownPositionAccelerometerCalibratorListener listener) {
        this(measurements, initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
                initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy, listener);
        try {
            setPosition(position);
        } catch (final LockedException ignore) {
            // never happens
        }
    }

    /**
     * Constructor.
     *
     * @param position       position where body kinematics measures have been taken.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBiasX   initial x-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialBiasY   initial y-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialBiasZ   initial z-coordinate of accelerometer bias to be used
     *                       to find a solution. This is expressed in meters per squared
     *                       second (m/s^2).
     * @param initialSx      initial x scaling factor.
     * @param initialSy      initial y scaling factor.
     * @param initialSz      initial z scaling factor.
     * @param initialMxy     initial x-y cross coupling error.
     * @param initialMxz     initial x-z cross coupling error.
     * @param initialMyx     initial y-x cross coupling error.
     * @param initialMyz     initial y-z cross coupling error.
     * @param initialMzx     initial z-x cross coupling error.
     * @param initialMzy     initial z-y cross coupling error.
     */
    public KnownPositionAccelerometerCalibrator(
            final ECEFPosition position, final boolean commonAxisUsed,
            final double initialBiasX, final double initialBiasY, final double initialBiasZ,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 731
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 1651
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 804
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 218
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 935
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 486
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 2091
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java 289
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 1009
@Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final Matrix currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 843
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 791
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 213
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 850
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 797
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 1814
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 1718
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java 480
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java 1834
com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java 1740
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 914
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 861
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java 285
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java 923
com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java 870
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 739
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1670
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 219
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1702
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 1655
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java 3540
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java 484
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java 3603
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 808
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1741
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java 290
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java 1773
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java 216
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 934
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 928
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 933
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java 483
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2085
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 2083
com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 2087
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java 287
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1002
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1000
com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java 1005
@Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return measurements.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(measurements.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2554
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 2792
final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param measurements list of body kinematics measurements with standard
     *                     deviations taken at different frames (positions, orientations
     *                     and velocities).
     * @param biasX        known x coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param biasY        known y coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param biasZ        known z coordinate of accelerometer bias expressed in meters per
     *                     squared second (m/s^2).
     * @param method       robust estimator method.
     * @return a robust accelerometer calibrator.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4226
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4465
final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias,
                    commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
                    commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
                    commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known accelerometer bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller than
     *                                  4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4260
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 4498
final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed,
                    listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed,
                    listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed,
                    listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
                    commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
                    commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param bias          known accelerometer bias.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided quality scores length is smaller than
     *                                  4 samples.
     */
    public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3745
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4399
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements,
            final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, listener);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, listener);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           list of body kinematics measurements taken at a given position with
     *                               different unknown orientations and containing the standard deviations
     *                               of accelerometer and gyroscope measurements.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3783
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4436
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           list of body kinematics measurements taken at a given position with
     *                               different unknown orientations and containing the standard deviations
     *                               of accelerometer and gyroscope measurements.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param listener               listener to be notified of events such as when estimation
     *                               starts, ends or its progress significantly changes.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores,
            final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3862
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4021
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4020
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4668
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4513
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4669
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3614
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4211
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, listener);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, listener);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 7 samples are
     *                       required, otherwise 10.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3650
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4249
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 7 samples are
     *                       required, otherwise 10.
     * @param listener       listener to be notified of events such as when estimation
     *                       starts, ends or its progress significantly changes.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3727
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3877
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3876
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4478
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4326
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4479
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param bias          known accelerometer bias. This must have length 3 and is
     *                      expressed in meters per squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 10
     *                                  samples.
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4271
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4925
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements,
            final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, listener);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, listener);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, listener);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, listener);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           list of body kinematics measurements taken at a given position with
     *                               different unknown orientations and containing the standard deviations
     *                               of accelerometer and gyroscope measurements.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4308
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4961
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           list of body kinematics measurements taken at a given position with
     *                               different unknown orientations and containing the standard deviations
     *                               of accelerometer and gyroscope measurements.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param listener               listener to be notified of events such as when estimation
     *                               starts, ends or its progress significantly changes.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4387
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4549
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4548
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5194
final double[] qualityScores, final Double groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial bias to find a solution.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5038
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5195
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm.
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is smaller
     *                                  than 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Acceleration groundTruthGravityNorm,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4130
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4722
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements,
            final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, listener);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, listener);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, listener);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, listener);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, listener);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 10 samples are
     *                       required, otherwise 13.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4164
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4756
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, commonAxisUsed);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   list of body kinematics measurements taken at a given position with
     *                       different unknown orientations and containing the standard deviations
     *                       of accelerometer and gyroscope measurements.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope. If true 10 samples are
     *                       required, otherwise 13.
     * @param listener       listener to be notified of events such as when estimation
     *                       starts, ends or its progress significantly changes.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than the minimum number of
     *                                  required samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4238
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4390
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4389
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4981
final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial bias to find a solution.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
     *                                  quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4830
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4982
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body kinematics measures have been taken.
     * @param measurements  collection of body kinematics measurements with standard
     *                      deviations taken at the same position with zero velocity
     *                      and unknown different orientations.
     * @param initialBias   initial accelerometer bias to be used to find a solution.
     *                      This must have length 3 and is expressed in meters per
     *                      squared second (m/s^2).
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if quality scores array is smaller than 13
     *                                  samples.
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3891
com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 2376
return evaluateGeneral(i, params);
            }
        });

        setInputData();

        fitter.fit();

        final var result = fitter.getA();

        final var m11 = result[0];
        final var m21 = result[1];
        final var m31 = result[2];

        final var m12 = result[3];
        final var m22 = result[4];
        final var m32 = result[5];

        final var m13 = result[6];
        final var m23 = result[7];
        final var m33 = result[8];

        final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4408
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 4704
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 4945
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5287
for (int i = 0, j = k; i < num; i++, j++) {
                    initial[j] = initG.getElementAtIndex(i);
                }

                return initial;
            }

            @Override
            public double evaluate(
                    final int i, final double[] point, final double[] params, final double[] derivatives)
                    throws EvaluationException {

                measAngularRateX = point[0];
                measAngularRateY = point[1];
                measAngularRateZ = point[2];

                fmeasX = point[3];
                fmeasY = point[4];
                fmeasZ = point[5];

                gradientEstimator.gradient(params, derivatives);

                return evaluateCommonAxisWitGDependentCrossBiases(params);
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 625
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 610
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java 1360
com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java 1330
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java 697
com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java 681
@Override
            public double getThreshold() {
                return threshold;
            }

            @Override
            public int getTotalSamples() {
                return sequences.size();
            }

            @Override
            public int getSubsetSize() {
                return preliminarySubsetSize;
            }

            @Override
            public void estimatePreliminarSolutions(
                    final int[] samplesIndices, final List<PreliminaryResult> solutions) {
                computePreliminarySolutions(samplesIndices, solutions);
            }

            @Override
            public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
                return computeError(sequences.get(i), currentEstimation);
            }

            @Override
            public boolean isReady() {
                return MSACRobustEasyGyroscopeCalibrator.super.isReady();
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 3956
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4734
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
                    accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4042
com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java 4832
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustEasyGyroscopeCalibrator(
                    sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2622
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 2856
final double biasX, final double biasY, final double biasZ,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param measurements list of body kinematics measurements with standard
     *                     deviations taken at different frames (positions, orientations
     *                     and velocities).
     * @param biasX        known x coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param biasY        known y coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param biasZ        known z coordinate of gyroscope bias expressed in radians per
     *                     second (rad/s).
     * @param method       robust estimator method.
     * @return a robust gyroscope calibrator.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final List<StandardDeviationFrameBodyKinematics> measurements,
            final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4278
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4515
final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, bias, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, bias, commonAxisUsed);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, bias, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, bias, commonAxisUsed);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, bias, commonAxisUsed);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   list of body kinematics measurements with standard
     *                       deviations taken at different frames (positions, orientations
     *                       and velocities).
     * @param bias           known gyroscope bias.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided quality scores length is smaller
     *                                  than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
            final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
            final double[] bias, final boolean commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4311
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 4548
final double[] bias, final boolean commonAxisUsed,
            final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, bias, commonAxisUsed, listener);
            case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, bias, commonAxisUsed, listener);
            case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    measurements, bias, commonAxisUsed, listener);
            case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, bias, commonAxisUsed, listener);
            default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
                    qualityScores, measurements, bias, commonAxisUsed, listener);
        };
    }

    /**
     * Creates a robust gyroscope calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param bias          known gyroscope bias.
     * @param method        robust estimator method.
     * @return a robust gyroscope calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided quality scores length is smaller
     *                                  than 6 samples.
     */
    public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3466
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4252
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
            final Matrix accelerometerMa, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
                    initialGg, accelerometerBias, accelerometerMa);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 3552
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java 4352
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
            final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
            final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
                    estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
            case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 4282
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 4432
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided quality
     *                                  scores length is smaller than 10 samples.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5016
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 5179
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron);
            case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron);
            default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param hardIron                           known hard-iron.
     * @param listener                           listener to handle events raised by this calibrator.
     * @param method                             robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4271
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4942
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, listener);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, listener);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, listener);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, listener);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4310
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4981
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4390
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4553
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4552
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5223
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5061
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5224
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores quality scores corresponding to each provided
     *                      measurement. The larger the score value the better
     *                      the quality of the sample.
     * @param position      position where body magnetic flux density measurements
     *                      have been taken.
     * @param measurements  collection of body magnetic flux density
     *                      measurements with standard deviation of
     *                      magnetometer measurements taken at the same
     *                      position with zero velocity and unknown different
     *                      orientations.
     * @param hardIron      known hard-iron.
     * @param listener      listener to handle events raised by this calibrator.
     * @param method        robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4794
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 4944
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided quality
     *                                  scores length is smaller than 10 samples.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final boolean commonAxisUsed, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5529
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 5692
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
            case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
            case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
            case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
            default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
                    qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores                      quality scores corresponding to each provided
     *                                           measurement. The larger the score value the better
     *                                           the quality of the sample.
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
     * @param measurements                       collection of body magnetic flux density
     *                                           measurements with standard deviation of
     *                                           magnetometer measurements taken at the same
     *                                           position with zero velocity and unknown different
     *                                           orientations.
     * @param initialHardIron                    initial hard-iron to find a solution.
     * @param listener                           listener to handle events raised by this calibrator.
     * @param method robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided magnetic flux norm value is negative,
     *                                  or if provided hard-iron array does not have length 3,
     *                                  or if provided quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
            final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4759
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5425
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements,
            final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, listener);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, listener);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, listener);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, listener);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, listener);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4798
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5464
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided quality scores length
     *                                  is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4878
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5041
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5040
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5707
final double[] qualityScores, final NEDPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5544
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5708
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
            final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 103
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 155
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 989
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1041
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1311
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1363
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1630
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1679
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 1980
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2029
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2073
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8848
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2117
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 8804
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
            final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
                oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVelocity  previous body velocity resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2333
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2385
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2431
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2477
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2757
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 2803
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECEF frame containing new body
     *                     position, velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 94
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 146
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 420
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 472
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ,
            final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 592
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 644
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 761
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 810
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 859
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 908
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 960
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1012
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1058
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1104
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1150
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 1196
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
            final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param result       instance where new estimated ECI frame containing new body position,
     *                     velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public void navigate(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17232
com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java 17383
final double angularRateX, final double angularRateY, final double angularRateZ,
            final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
        try {
            navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
                    oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
                    fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
        } catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
            // never happens
        }
    }

    /**
     * Runs precision local-navigation-frame inertial navigation equations.
     * NOTE: only the attitude update and specific force frame transformation
     * phases are precise.
     *
     * @param timeInterval      time interval between epochs expressed in seconds (s).
     * @param oldFrame          previous NED frame containing body position, velocity and
     *                          coordinate transformation matrix.
     * @param fx                specific force x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fy                specific force y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param fz                specific force z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval.
     * @param angularRateX      angular rate x-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateY      angular rate y-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param angularRateZ      angular rate z-coordinate of body frame with respect ECEF frame,
     *                          resolved along body-frame axes, averaged over time interval and
     *                          expressed in radians per second (rad/s).
     * @param result            instance where new estimated NED frame containing new body position,
     *                          velocity and coordinate transformation matrix will be stored.
     * @throws InertialNavigatorException if navigation fails due to numerical instabilities.
     */
    public static void navigateNED(
            final double timeInterval, final NEDFrame oldFrame,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
File Line
com/irurueta/navigation/inertial/calibration/AccelerationFixer.java 1174
com/irurueta/navigation/inertial/calibration/AngularRateFixer.java 1343
com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java 1257
final double myz, final double mzx, final double mzy, final double[] result) throws AlgebraException {

        crossCouplingErrors.setElementAt(0, 0, sx);
        crossCouplingErrors.setElementAt(1, 1, sy);
        crossCouplingErrors.setElementAt(2, 2, sz);
        crossCouplingErrors.setElementAt(0, 1, mxy);
        crossCouplingErrors.setElementAt(0, 2, mxz);
        crossCouplingErrors.setElementAt(1, 0, myx);
        crossCouplingErrors.setElementAt(1, 2, myz);
        crossCouplingErrors.setElementAt(2, 0, mzx);
        crossCouplingErrors.setElementAt(2, 1, mzy);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java 5107
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java 3676
com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java 5012
final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];

                // upper diagonal cross coupling errors M
                var k = 0;
                for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
                    for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
                        if (i <= j) {
                            initial[k] = initialM.getElementAt(i, j);
                            k++;
                        }
                    }
                }

                return initial;
            }

            @Override
            public double evaluate(final int i, final double[] point, final double[] params,
                                   final double[] derivatives) throws EvaluationException {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 3784
com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java 4052
result[2] = bz + ftruez + sz * ftruez;

                jacobian.setElementAt(0, 0, 1.0);
                jacobian.setElementAt(0, 1, 0.0);
                jacobian.setElementAt(0, 2, 0.0);
                jacobian.setElementAt(0, 3, ftruex);
                jacobian.setElementAt(0, 4, 0.0);
                jacobian.setElementAt(0, 5, 0.0);
                jacobian.setElementAt(0, 6, ftruey);
                jacobian.setElementAt(0, 7, ftruez);
                jacobian.setElementAt(0, 8, 0.0);

                jacobian.setElementAt(1, 0, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 818
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java 770
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java 763
com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java 826
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value, the
     * algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @return stop threshold to stop the algorithm prematurely when a certain
     * accuracy has been reached.
     */
    public double getStopThreshold() {
        return stopThreshold;
    }

    /**
     * Sets threshold to be used to keep the algorithm iterating in case that
     * best estimated threshold using median of residuals is not small enough.
     * Once a solution is found that generates a threshold below this value,
     * the algorithm will stop.
     * The stop threshold can be used to prevent the LMedS algorithm to iterate
     * too many times in cases where samples have a very similar accuracy.
     * For instance, in cases where proportion of outliers is very small (close
     * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
     * iterate for a long time trying to find the best solution when indeed
     * there is no need to do that if a reasonable threshold has already been
     * reached.
     * Because of this behaviour the stop threshold can be set to a value much
     * lower than the one typically used in RANSAC, and yet the algorithm could
     * still produce even smaller thresholds in estimated results.
     *
     * @param stopThreshold stop threshold to stop the algorithm prematurely
     *                      when a certain accuracy has been reached.
     * @throws IllegalArgumentException if provided value is zero or negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setStopThreshold(final double stopThreshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (stopThreshold <= MIN_STOP_THRESHOLD) {
            throw new IllegalArgumentException();
        }

        this.stopThreshold = stopThreshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java 795
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 910
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 858
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java 280
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 917
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 864
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java 692
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java 806
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java 679
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java 1737
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java 286
com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java 1769
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1003
com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 995
inliersData = null;
            innerEstimator.setConfidence(confidence);
            innerEstimator.setMaxIterations(maxIterations);
            innerEstimator.setProgressDelta(progressDelta);
            final var preliminaryResult = innerEstimator.estimate();
            inliersData = innerEstimator.getInliersData();

            attemptRefine(preliminaryResult);

            if (listener != null) {
                listener.onCalibrateEnd(this);
            }

        } catch (final com.irurueta.numerical.LockedException e) {
            throw new LockedException(e);
        } catch (final com.irurueta.numerical.NotReadyException e) {
            throw new NotReadyException(e);
        } catch (final RobustEstimatorException e) {
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java 794
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java 746
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing bias, scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java 740
com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java 801
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
    }

    /**
     * Returns threshold to determine whether samples are inliers or not.
     *
     * @return threshold to determine whether samples are inliers or not.
     */
    public double getThreshold() {
        return threshold;
    }

    /**
     * Sets threshold to determine whether samples are inliers or not.
     *
     * @param threshold threshold to be set.
     * @throws IllegalArgumentException if provided value is equal or less than
     *                                  zero.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setThreshold(final double threshold) throws LockedException {
        if (running) {
            throw new LockedException();
        }
        if (threshold <= MIN_THRESHOLD) {
            throw new IllegalArgumentException();
        }
        this.threshold = threshold;
    }

    /**
     * Estimates accelerometer calibration parameters containing scale factors
     * and cross-coupling errors.
     *
     * @throws LockedException      if calibrator is currently running.
     * @throws NotReadyException    if calibrator is not ready.
     * @throws CalibrationException if estimation fails for numerical reasons.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void calibrate() throws LockedException, NotReadyException, CalibrationException {
        if (running) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java 80
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java 83
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java 85
com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java 89
public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body kinematics measurements taken at different
     * frames (positions, orientations and velocities) and containing the standard
     * deviations of accelerometer and gyroscope measurements.
     * If a single device IMU needs to be calibrated, typically all measurements are
     * taken at the same position, with zero velocity and multiple orientations.
     * However, if we just want to calibrate a given IMU model (e.g. obtain
     * an average and less precise calibration for the IMU of a given phone model),
     * we could take measurements collected throughout the planet at multiple positions
     * while the phone remains static (e.g. while charging), hence each measurement
     * position will change, velocity will remain zero and orientation will be
     * typically constant at horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyKinematics> measurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownBiasAndFrameAccelerometerCalibratorListener listener;
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 3943
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4745
final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias. This must have length 3 and is expressed
     *                               in meters per squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4099
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java 4591
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param bias                   known accelerometer bias.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3802
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4551
final double[] bias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias. This must have length 3 and is
     *                       expressed in meters per squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 3949
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java 4403
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
            final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, bias);
            case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
            default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, bias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param bias           known accelerometer bias.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (7 or 10).
     */
    public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4470
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5271
final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial accelerometer bias to be used to find a solution.
     *                               This must have length 3 and is expressed in meters per
     *                               squared second (m/s^2).
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 4627
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java 5118
final Matrix initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
                    groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
                    qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores          quality scores corresponding to each provided
     *                               measurement. The larger the score value the better
     *                               the quality of the sample.
     * @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
     *                               squared second (m/s^2).
     * @param measurements           collection of body kinematics measurements with standard
     *                               deviations taken at the same position with zero velocity
     *                               and unknown different orientations.
     * @param commonAxisUsed         indicates whether z-axis is assumed to be common for
     *                               accelerometer and gyroscope.
     * @param initialBias            initial bias to find a solution.
     * @param listener               listener to handle events raised by this calibrator.
     * @param method                 robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1 or
     *                                  if provided gravity norm value is negative or
     *                                  if provided quality scores length is
     *                                  smaller than 10 or 13 samples.
     */
    public static RobustKnownGravityNormAccelerometerCalibrator create(
            final double[] qualityScores, final Double groundTruthGravityNorm,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4314
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 5054
final double[] initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial accelerometer bias to be used to find a solution.
     *                       This must have length 3 and is expressed in meters per
     *                       squared second (m/s^2).
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias array does not have length 3
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4465
com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java 4906
final Matrix initialBias, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
                    position, measurements, commonAxisUsed, initialBias);
            case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
            default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialBias);
        };
    }

    /**
     * Creates a robust accelerometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body kinematics measures have been taken.
     * @param measurements   collection of body kinematics measurements with standard
     *                       deviations taken at the same position with zero velocity
     *                       and unknown different orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common for
     *                       accelerometer and gyroscope.
     * @param initialBias    initial bias to find a solution.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust accelerometer calibrator.
     * @throws IllegalArgumentException if provided bias matrix is not 3x1
     *                                  or if provided quality scores length is
     *                                  smaller than the minimum number of required
     *                                  samples (10 or 13).
     */
    public static RobustKnownPositionAccelerometerCalibrator create(
            final double[] qualityScores, final ECEFPosition position,
File Line
com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java 4257
com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java 5621
final var gradientEstimator = new GradientEstimator(params -> evaluateCommonAxis(i, params));

        final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
        initialM.add(getInitialMg());

        // Force initial M to be upper diagonal
        initialM.setElementAt(1, 0, 0.0);
        initialM.setElementAt(2, 0, 0.0);
        initialM.setElementAt(2, 1, 0.0);

        final var invInitialM = Utils.inverse(initialM);
        final var initialBg = getInitialBiasAsMatrix();
        final var initialB = invInitialM.multiplyAndReturnNew(initialBg);

        fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
            @Override
            public int getNumberOfDimensions() {
                // Before and after normalized gravity versors
                return 2 * BodyKinematics.COMPONENTS;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4531
com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java 4801
result[2] = bz + btruez + sz * btruez;

                jacobian.setElementAt(0, 0, 1.0);
                jacobian.setElementAt(0, 1, 0.0);
                jacobian.setElementAt(0, 2, 0.0);
                jacobian.setElementAt(0, 3, btruex);
                jacobian.setElementAt(0, 4, 0.0);
                jacobian.setElementAt(0, 5, 0.0);
                jacobian.setElementAt(0, 6, btruey);
                jacobian.setElementAt(0, 7, btruez);
                jacobian.setElementAt(0, 8, 0.0);

                jacobian.setElementAt(1, 0, 0.0);
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 812
com/irurueta/navigation/inertial/calibration/magnetometer/KnownMagneticFluxDensityNormMagnetometerCalibrator.java 855
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java 1259
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java 1263
}

    /**
     * Sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made, expressed in Teslas (T).
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm)
            throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }

        internalSetGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
    }

    /**
     * Sets ground truth magnetic flux density norm to be expected at location where
     * measurements have been made.
     *
     * @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
     * @throws IllegalArgumentException if provided value is negative.
     * @throws LockedException          if calibrator is currently running.
     */
    public void setGroundTruthMagneticFluxDensityNorm(final MagneticFluxDensity groundTruthMagneticFluxDensityNorm)
            throws LockedException {
        if (isRunning()) {
            throw new LockedException();
        }
        if (groundTruthMagneticFluxDensityNorm != null) {
            internalSetGroundTruthMagneticFluxDensityNorm(MagneticFluxDensityConverter.convert(
                    groundTruthMagneticFluxDensityNorm.getValue().doubleValue(),
                    groundTruthMagneticFluxDensityNorm.getUnit(),
                    MagneticFluxDensityUnit.TESLA));
        } else {
            internalSetGroundTruthMagneticFluxDensityNorm(null);
        }
    }

    /**
     * Indicates whether calibrator is ready to start.
     *
     * @return true if calibrator is ready, false otherwise.
     */
    @Override
    public boolean isReady() {
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java 87
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java 86
public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;

    /**
     * Indicates that by default preliminary solutions are refined.
     */
    public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;

    /**
     * Default robust estimator method when none is provided.
     */
    public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;

    /**
     * Indicates that result is refined by default using a non-linear calibrator
     * (which uses a Levenberg-Marquardt fitter).
     */
    public static final boolean DEFAULT_REFINE_RESULT = true;

    /**
     * Indicates that covariance is kept by default after refining result.
     */
    public static final boolean DEFAULT_KEEP_COVARIANCE = true;

    /**
     * Default amount of progress variation before notifying a change in estimation progress.
     * By default this is set to 5%.
     */
    public static final float DEFAULT_PROGRESS_DELTA = 0.05f;

    /**
     * Minimum allowed value for progress delta.
     */
    public static final float MIN_PROGRESS_DELTA = 0.0f;

    /**
     * Maximum allowed value for progress delta.
     */
    public static final float MAX_PROGRESS_DELTA = 1.0f;

    /**
     * Constant defining default confidence of the estimated result, which is
     * 99%. This means that with a probability of 99% estimation will be
     * accurate because chosen sub-samples will be inliers.
     */
    public static final double DEFAULT_CONFIDENCE = 0.99;

    /**
     * Default maximum allowed number of iterations.
     */
    public static final int DEFAULT_MAX_ITERATIONS = 5000;

    /**
     * Minimum allowed confidence value.
     */
    public static final double MIN_CONFIDENCE = 0.0;

    /**
     * Maximum allowed confidence value.
     */
    public static final double MAX_CONFIDENCE = 1.0;

    /**
     * Minimum allowed number of iterations.
     */
    public static final int MIN_ITERATIONS = 1;

    /**
     * Contains a list of body magnetic flux density measurements taken
     * at different frames (positions and orientations) and containing the
     * standard deviation of magnetometer measurements.
     * If a single device magnetometer needs to be calibrated, typically all
     * measurements are taken at the same position, with zero velocity and
     * multiple orientations.
     * However, if we just want to calibrate a given magnetometer model (e.g.
     * obtain an average and less precise calibration for the magnetometer of
     * a given phone model), we could take measurements collected throughout
     * the planet at multiple positions while the phone remains static (e.g.
     * while charging), hence each measurement position will change, velocity
     * will remain zero and orientation will be typically constant at
     * horizontal orientation while the phone remains on a
     * flat surface.
     */
    protected List<StandardDeviationFrameBodyMagneticFluxDensity> measurements;

    /**
     * Listener to be notified of events such as when calibration starts, ends or its
     * progress significantly changes.
     */
    protected RobustKnownFrameMagnetometerCalibratorListener listener;
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4473
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5302
final double[] hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 4631
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java 5144
final Matrix hardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, hardIron);
            case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
            default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, hardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores  quality scores corresponding to each provided
     *                       measurement. The larger the score value the better
     *                       the quality of the sample.
     * @param position       position where body magnetic flux density measurements
     *                       have been taken.
     * @param measurements   collection of body magnetic flux density
     *                       measurements with standard deviation of
     *                       magnetometer measurements taken at the same
     *                       position with zero velocity and unknown different
     *                       orientations.
     * @param commonAxisUsed indicates whether z-axis is assumed to be common
     *                       for the accelerometer, gyroscope and magnetometer.
     * @param hardIron       known hard-iron.
     * @param listener       listener to handle events raised by this calibrator.
     * @param method         robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 4961
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5786
final double[] initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron array does
     *                                  not have length 3 or if provided
     *                                  quality scores length is smaller
     *                                  than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5119
com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java 5627
final Matrix initialHardIron, final RobustEstimatorMethod method) {
        return switch (method) {
            case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    position, measurements, commonAxisUsed, initialHardIron);
            case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
            default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
                    qualityScores, position, measurements, commonAxisUsed, initialHardIron);
        };
    }

    /**
     * Creates a robust magnetometer calibrator.
     *
     * @param qualityScores   quality scores corresponding to each provided
     *                        measurement. The larger the score value the better
     *                        the quality of the sample.
     * @param position        position where body magnetic flux density measurements
     *                        have been taken.
     * @param measurements    collection of body magnetic flux density
     *                        measurements with standard deviation of
     *                        magnetometer measurements taken at the same
     *                        position with zero velocity and unknown different
     *                        orientations.
     * @param commonAxisUsed  indicates whether z-axis is assumed to be common
     *                        for the accelerometer, gyroscope and magnetometer.
     * @param initialHardIron initial hard-iron to find a solution.
     * @param listener        listener to handle events raised by this calibrator.
     * @param method          robust estimator method.
     * @return a robust magnetometer calibrator.
     * @throws IllegalArgumentException if provided hard-iron matrix is not
     *                                  3x1 or if provided quality scores
     *                                  length is smaller than 10 samples.
     */
    public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
            final double[] qualityScores, final NEDPosition position,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10209
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11143
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10211
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11818
final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10212
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12185
final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 10263
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11197
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11481
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12551
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11535
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12605
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldPosition  previous cartesian position resolved along ECEF-frame axes.
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 11819
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12998
final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
File Line
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12184
com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java 12997
final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECEFFrame();
        navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECEF-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECEF
     *                     frame, resolved along ECEF-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECEF-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECEF frame,
     *                     resolved along ECEF-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECEF frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECEF-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECEFFrame navigateECEFAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4822
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5164
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4824
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5523
final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4825
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5626
final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4876
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5218
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final double timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 4878
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5574
final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final BodyKinematics kinematics) throws InertialNavigatorException,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5344
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5728
final double timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5398
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5782
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param kinematics   body kinematics containing specific forces and angular rates applied to
     *                     the body.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final double timeInterval, final double oldX, final double oldY, final double oldZ,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5524
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5929
final double angularRateX, final double angularRateY, final double angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final Acceleration fx, final Acceleration fy, final Acceleration fz,
            final double angularRateX, final double angularRateY, final double angularRateZ)
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5625
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5928
final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs.
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes and expressed in meters (m).
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldVx        previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVy        previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param oldVz        previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes and expressed in meters per second (m/s).
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval.
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final Time timeInterval, final double oldX, final double oldY, final double oldZ,
            final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
            final double fx, final double fy, final double fz,
File Line
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5676
com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java 5976
final double fx, final double fy, final double fz,
            final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
            throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
        final var result = new ECIFrame();
        navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
                angularRateX, angularRateY, angularRateZ, result);
        return result;
    }

    /**
     * Runs precision ECI-frame inertial navigation equations.
     *
     * @param timeInterval time interval between epochs expressed in seconds (s).
     * @param oldX         previous cartesian x-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldY         previous cartesian y-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldZ         previous cartesian z-coordinate position of body frame with respect ECI
     *                     frame, resolved along ECI-frame axes.
     * @param oldC         previous body-to-ECI-frame coordinate transformation.
     * @param oldSpeedX    previous velocity x-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedY    previous velocity y-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param oldSpeedZ    previous velocity z-coordinate of body frame with respect ECI frame,
     *                     resolved along ECI-frame axes.
     * @param fx           specific force x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fy           specific force y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param fz           specific force z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in meters per squared second (m/s^2).
     * @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
     *                     resolved along body-frame axes, averaged over time interval and
     *                     expressed in radians per second (rad/s).
     * @return estimated ECI frame containing new body position, velocity and coordinate
     * transformation matrix.
     * @throws InertialNavigatorException                    if navigation fails due to numerical instabilities.
     * @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
     *                                                       body-to-ECI-frame coordinate transformation matrix are
     *                                                       invalid.
     */
    public static ECIFrame navigateECIAndReturnNew(
            final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
            final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
            final double fx, final double fy, final double fz,